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1 

INTRO D UCTION 


Jr^,obotics is a relatively young field of modern technology that crosses tra- 
ditional engineering boundaries. Understanding the complexity of robots and 
their applications requires knowledge of electrical engineering, mechanical engi- 
neering, systems and industrial engineering, computer science, economics, and 
mathematics. New disciplines of engineering, such as manufacturing engineer- 
ing, applications engineering, and knowledge engineering have emerged to deal 
with the complexity of the field of robotics and factory automation. 

This book is concerned with fundamentals of robotics, including kinematics, 
dynamics, motion planning, computer vision, and control. Our goal is 
to provide a complete introduction to the most important concepts in these 
subjects as applied to industrial robot manipulators, mobile robots, and other 
mechanical systems. A complete treatment of the discipline of robotics would 
require several volumes. Nevertheless, at the present time, the majority of robot 
applications deal with industrial robot arms operating in structured factory 
environments so that a first introduction to the subject of robotics must include 
a rigorous treatment of the topics in this text. 

The term robot was first introduced into our vocabulary by the Czech play- 
wright Karel Capek in his 1920 play Rossum's Universal Robots, the word 
robota being the Czech word for work. Since then the term has been applied to 
a great variety of mechanical devices, such as teleoperators, underwater vehi- 
cles, autonomous land rovers, etc. Virtually anything that operates with some 
degree of autonomy, usually under computer control, has at some point been 
called a robot. In this text the term robot will mean a computer controlled 
industrial manipulator of the type shown in Figure 0 This type of robot is 
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Fig. 1.1 The ABB IRB6600 Robot. Photo courtesy of ABB. 


essentially a mechanical arm operating under computer control. Such devices, 
though far from the robots of science fiction, are nevertheless extremely com- 
plex electro-mechanical systems whose analytical description requires advanced 
methods, presenting many challenging and interesting research problems. 

An official definition of such a robot comes from the Robot Institute of 
America (RIA): A robot is a reprogrammable multifunctional manipulator 
designed to move material, parts, tools, or specialized devices through variable 
programmed motions for the performance of a variety of tasks. 

The key element in the above definition is the reprogrammability of robots. 
It is the computer brain that gives the robot its utility and adaptability. The 
so-called robotics revolution is, in fact, part of the larger computer revolution. 

Even this restricted version of a robot has several features that make it at- 
tractive in an industrial environment. Among the advantages often cited in 
favor of the introduction of robots are decreased labor costs, increased preci- 
sion and productivity, increased flexibility compared with specialized machines, 
and more humane working conditions as dull, repetitive, or hazardous jobs are 
performed by robots. 

The robot, as we have defined it, was born out of the marriage of two ear- 
lier technologies: teleoperators and numerically controlled milling ma- 
chines. Teleoperators, or master-slave devices, were developed during the sec- 
ond world war to handle radioactive materials. Computer numerical control 
(CNC) was developed because of the high precision required in the machining 
of certain items, such as components of high performance aircraft. The first 
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robots essentially combined the mechanical linkages of the teleoperator with 
the autonomy and programmability of CNC machines. 

The first successful applications of robot manipulators generally involved 
some sort of material transfer, such as injection molding or stamping, where 
the robot merely attends a press to unload and either transfer or stack the 
finished parts. These first robots could be programmed to execute a sequence 
of movements, such as moving to a location A, closing a gripper, moving to a 
location B, etc., but had no external sensor capability. More complex appli- 
cations, such as welding, grinding, deburring, and assembly require not only 
more complex motion but also some form of external sensing such as vision, 
tactile, or force-sensing, due to the increased interaction of the robot with its 
environment. 

It should be pointed out that the important applications of robots are by no 
means limited to those industrial jobs where the robot is directly replacing a 
human worker. There are many other applications of robotics in areas where 
the use of humans is impractical or undesirable. Among these are undersea and 
planetary exploration, satellite retrieval and repair, the defusing of explosive 
devices, and work in radioactive environments. Finally, prostheses, such as 
artificial limbs, are themselves robotic devices requiring methods of analysis 
and design similar to those of industrial manipulators. 


1.1 MATHEMATICAL MODELING OF ROBOTS 

While robots are themselves mechanical systems, in this text we will be pri- 
marily concerned with developing and manipulating mathematical models for 
robots. In particular, we will develop methods to represent basic geometric as- 
pects of robotic manipulation, dynamic aspects of manipulation, and the various 
sensors available in modern robotic systems. Equipped with these mathematical 
models, we will be able to develop methods for planning and controlling robot 
motions to perform specified tasks. Here we describe some of the basic ideas 
that are common in developing mathematical models for robot manipulators. 

1.1.1 Symbolic Representation of Robots 

Robot Manipulators are composed of links connected by joints to form a kine- 
matic chain. Joints are typically rotary (revolute) or linear (prismatic). A 
revolute joint is like a hinge and allows relative rotation between two links. A 
prismatic joint allows a linear relative motion between two links. We denote 
revolute joints by R and prismatic joints by P, and draw them as shown in 
Figure |L2} For example, a three-link arm with three revolute joints is an RRR 
arm. 

Each joint represents the interconnection between two links. We denote the 
axis of rotation of a revolute joint, or the axis along which a prismatic joint 
translates by Zi if the joint is the interconnection of links i and i + 1. The 
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Fig. 1.2 Symbolic representation of robot joints. 


joint variables, denoted by 9 for a revolute joint and d for the prismatic joint, 
represent the relative displacement between adjacent links. We will make this 
precise in Chapter [3| 

1.1.2 The Configuration Space 

A configuration of a manipulator is a complete specification of the location 
of every point on the manipulator. The set of all possible configurations is 
called the configuration space. In our case, if we know the values for the 
joint variables (i.e., the joint angle for revolute joints, or the joint offset for 
prismatic joints), then it is straightforward to infer the position of any point 
on the manipulator, since the individual links of the manipulator are assumed 
to be rigid, and the base of the manipulator is assumed to be fixed. Therefore, 
in this text, we will represent a configuration by a set of values for the joint 
variables. We will denote this vector of values by q , and say that the robot 
is in configuration q when the joint variables take on the values q\ ■ ■ ■ q n , with 
qi = 9i for a revolute joint and g, = d\ for a prismatic joint. 

An object is said to have n degrees-of-freedom (DOF) if its configuration 
can be minimally specified by n parameters. Thus, the number of DOF is equal 
to the dimension of the configuration space. For a robot manipulator, the num- 
ber of joints determines the number DOF. A rigid object in three-dimensional 
space has six DOF: three for positioning and three for orientation (e.g., roll, 
pitch and yaw angles). Therefore, a manipulator should typically possess at 
least six independent DOF. With fewer than six DOF the arm cannot reach 
every point in its work environment with arbitrary orientation. Certain appli- 
cations such as reaching around or behind obstacles may require more than six 
DOF. A manipulator having more than six links is referred to as a kinemat- 
ically redundant manipulator. The difficulty of controlling a manipulator 
increases rapidly with the number of links. 
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1.1.3 The State Space 

A configuration provides an instantaneous description of the geometry of a 
manipulator, but says nothing about its dynamic response. In contrast, the 
state of the manipulator is a set of variables that, together with a description 
of the manipulator’s dynamics and input, are sufficient to determine any future 
state of the manipulator. The state space is the set of all possible states. 
In the case of a manipulator arm, the dynamics are Newtonian, and can be 
specified by generalizing the familiar equation F = ma. Thus, a state of the 
manipulator can be specified by giving the values for the joint variables q and 
for joint velocities q (acceleration is related to the derivative of joint velocities). 
We typically represent the state as a vector x = (q, q) T . The dimension of the 
state space is thus 2 n if the system has n DOF. 

1.1.4 The Workspace 

The workspace of a manipulator is the total volume swept out by the end- 
effector as the manipulator executes all possible motions. The workspace is 
constrained by the geometry of the manipulator as well as mechanical con- 
straints on the joints. For example, a revolute joint may be limited to less 
than a full 360° of motion. The workspace is often broken down into a reach- 
able workspace and a dexterous workspace. The reachable workspace is 
the entire set of points reachable by the manipulator, whereas the dexterous 
workspace consists of those points that the manipulator can reach with an ar- 
bitrary orientation of the end-effector. Obviously the dexterous workspace is a 
subset of the reachable workspace. The workspaces of several robots are shown 
later in this chapter. 


1.2 ROBOTS AS MECHANICAL DEVICES 

There are a number of physical aspects of robotic manipulators that we will not 
necessarily consider when developing our mathematical models. These include 
mechanical aspects (e.g., how are the joints actually implemented), accuracy 
and repeatability, and the tooling attached at the end effector. In this section, 
we briefly describe some of these. 

1.2.1 Classification of Robotic Manipulators 

Robot manipulators can be classified by several criteria, such as their power 
source, or way in which the joints are actuated, their geometry, or kinematic 
structure, their intended application area, or their method of control. Such 
classification is useful primarily in order to determine which robot is right for 
a given task. For example, a hydraulic robot would not be suitable for food 
handling or clean room applications. We explain this in more detail below. 
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Power Source. Typically, robots are either electrically, hydraulically, or pneu- 
matically powered. Hydraulic actuators are unrivaled in their speed of response 
and torque producing capability. Therefore hydraulic robots are used primar- 
ily for lifting heavy loads. The drawbacks of hydraulic robots are that they 
tend to leak hydraulic fluid, require much more peripheral equipment (such as 
pumps, which require more maintenance), and they are noisy. Robots driven 
by DC- or AC-servo motors are increasingly popular since they are cheaper, 
cleaner and quieter. Pneumatic robots are inexpensive and simple but cannot 
be controlled precisely. As a result, pneumatic robots are limited in their range 
of applications and popularity. 

Application Area. Robots are often classified by application into assembly 
and non-assembly robots. Assembly robots tend to be small, electrically 
driven and either revolute or SCARA (described below) in design. The main 
nonassembly application areas to date have been in welding, spray painting, 
material handling, and machine loading and unloading. 

Method of Control. Robots are classified by control method into servo and 
non-servo robots. The earliest robots were non-servo robots. These robots 
are essentially open-loop devices whose movement is limited to predetermined 
mechanical stops, and they are useful primarily for materials transfer. In fact, 
according to the definition given previously, fixed stop robots hardly qualify 
as robots. Servo robots use closed-loop computer control to determine their 
motion and are thus capable of being truly multifunctional, reprogrammable 
devices. 

Servo controlled robots are further classified according to the method that the 
controller uses to guide the end-effector. The simplest type of robot in this class 
is the point-to-point robot. A point-to-point robot can be taught a discrete 
set of points but there is no control on the path of the end-effector in between 
taught points. Such robots are usually taught a series of points with a teach 
pendant. The points are then stored and played back. Point-to-point robots 
are severely limited in their range of applications. In continuous path robots, 
on the other hand, the entire path of the end-effector can be controlled. For 
example, the robot end-effector can be taught to follow a straight line between 
two points or even to follow a contour such as a welding seam. In addition, the 
velocity and/or acceleration of the end-effector can often be controlled. These 
are the most advanced robots and require the most sophisticated computer 
controllers and software development. 

Geometry. Most industrial manipulators at the present time have six or fewer 
degrees-of-freedom. These manipulators are usually classified kinematically on 
the basis of the first three joints of the arm, with the wrist being described 
separately. The majority of these manipulators fall into one of five geometric 
types: articulated (RRR), spherical (RRP), SCARA (RRP), cylindri- 
cal (RPP) , or Cartesian (PPP). We discuss each of these below. 
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Each of these five manipulator arms are serial link robots. A sixth distinct 
class of manipulators consists of the so-called parallel robot. In a parallel 
manipulator the links are arranged in a closed rather than open kinematic 
chain. Although we include a brief discussion of parallel robots in this chapter, 
their kinematics and dynamics are more difficult to derive than those of serial 
link robots and hence are usually treated only in more advanced texts. 

1.2.2 Robotic Systems 


A robot manipulator should be viewed as more than just a series of mechanical 
linkages. The mechanical arm is just one component in an overall Robotic 



Fig. 1.3 Components of a robotic system. 


source, end-of-arm tooling, external and internal sensors, computer 
interface, and control computer. Even the programmed software should 
be considered as an integral part of the overall system, since the manner in 
which the robot is programmed and controlled can have a major impact on its 
performance and subsequent range of applications. 

1.2.3 Accuracy and Repeatability 

The accuracy of a manipulator is a measure of how close the manipulator can 
come to a given point within its workspace. Repeatability is a measure of 
how close a manipulator can return to a previously taught point. The primary 
method of sensing positioning errors in most cases is with position encoders 
located at the joints, either on the shaft of the motor that actuates the joint or 
on the joint itself. There is typically no direct measurement of the end-effector 
position and orientation. One must rely on the assumed geometry of the manip- 
ulator and its rigidity to infer (i.e., to calculate) the end-effector position from 
the measured joint positions. Accuracy is affected therefore by computational 
errors, machining accuracy in the construction of the manipulator, flexibility 
effects such as the bending of the links under gravitational and other loads, 
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gear backlash, and a host of other static and dynamic effects. It is primarily 
for this reason that robots are designed with extremely high rigidity. Without 
high rigidity, accuracy can only be improved by some sort of direct sensing of 
the end-effector position, such as with vision. 

Once a point is taught to the manipulator, however, say with a teach pen- 
dant, the above effects are taken into account and the proper encoder values 
necessary to return to the given point are stored by the controlling computer. 
Repeatability therefore is affected primarily by the controller resolution. Con- 
troller resolution means the smallest increment of motion that the controller 
can sense. The resolution is computed as the total distance traveled by the 
tip divided by 2 n , where n is the number of bits of encoder accuracy. In this 
context, linear axes, that is, prismatic joints, typically have higher resolution 
than revolute joints, since the straight line distance traversed by the tip of a 
linear axis between two points is less than the corresponding arc length traced 
by the tip of a rotational link. 

In addition, as we will see in later chapters, rotational axes usually result 
in a large amount of kinematic and dynamic coupling among the links with a 
resultant accumulation of errors and a more difficult control problem. One may 
wonder then what the advantages of revolute joints are in manipulator design. 
The answer lies primarily in the increased dexterity and compactness of revolute 



Fig. 1.4 Linear vs. rotational link motion. 


a rotational link can be made much smaller than a link with linear motion. Thus 
manipulators made from revolute joints occupy a smaller working volume than 
manipulators with linear axes. This increases the ability of the manipulator to 
work in the same space with other robots, machines, and people. At the same 
time revolute joint manipulators are better able to maneuver around obstacles 
and have a wider range of possible applications. 

1.2.4 Wrists and End- Effectors 

The joints in the kinematic chain between the arm and end effector are referred 
to as the wrist. The wrist joints are nearly always all revolute. It is increasingly 
common to design manipulators with spherical wrists, by which we mean 
wrists whose three joint axes intersect at a common point. The spherical wrist 
is represented symbolically in Figure [T3| 
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The spherical wrist greatly simplifies the kinematic analysis, effectively al- 
lowing one to decouple the positioning and orientation of the end effector. Typ- 
ically therefore, the manipulator will possess three degrees-of-freedom for posi- 
tion, which are produced by three or more joints in the arm. The number of 
degrees-of-freedom for orientation will then depend on the degrees-of-freedom 
of the wrist. It is common to find wrists having one, two, or three degrees-of- 
freedom depending of the application. For example, the SCARA robot shown 
in Figure [T. 14| has four degrees-of-freedom: three for the arm, and one for the 
wrist, which has only a rotation about the final z-axis. 

It has been said that a robot is only as good as its hand or end-effector. 
The arm and wrist assemblies of a robot are used primarily for positioning 
the end-effector and any tool it may carry. It is the end-effector or tool that 
actually performs the work. The simplest type of end-effectors are grippers, 
which usually are capable of only two actions, opening and closing. While 
this is adequate for materials transfer, some parts handling, or gripping simple 
tools, it is not adequate for other tasks such as welding, assembly, grinding, 
etc. A great deal of research is therefore devoted to the design of special pur- 
pose end-effectors as well as to tools that can be rapidly changed as the task 
dictates. There is also much research on the development of anthropomorphic 
hands. Such hands have been developed both for prosthetic use and for use 
in manufacturing. Since we are concerned with the analysis and control of the 
manipulator itself and not in the particular application or end-effector, we will 
not discuss end-effector design or the study of grasping and manipulation. 


1.3 COMMON KINEMATIC ARRANGEMENTS OF MANIPULATORS 

Although there are many possible ways use prismatic and revolute joints to 
construct kinematic chains, in practice only a few of these are commonly used. 
Here we briefly describe several arrangements that are most typical. 
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1.3.1 Articulated manipulator (RRR) 

The articulated manipulator is also called a revolute, or anthropomorphic 
manipulator. The ABB IRB1400 articulated arm is shown in Figure [L6| A 



Fig. 1.6 The ABB IRB1400 Robot. Photo courtesy of ABB. 

common revolute joint design is the parallelogram linkage such as the Mo- 
toman SK16, shown in Figure |1.7| In both of these arrangements joint axis 



Fig. 1.7 The Motoman SK16 manipulator. 


Z2 is parallel to z\ and both Z\ and z 2 are perpendicular to zq . This kind of 
manipulator is known as an elbow manipulator. The structure and terminology 
associated with the elbow manipulator are shown in Figure 1.8 Its workspace 


is shown in Figure 1.9 


The revolute manipulator provides for relatively large freedom of movement 
in a compact space. The parallelogram linkage, although typically less dexterous 
than the elbow manipulator manipulator, nevertheless has several advantages 
that make it an attractive and popular design. The most notable feature of the 
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Fig. 1.9 Workspace of the elbow manipulator. 


parallelogram linkage manipulator is that the actuator for joint 3 is located on 
link 1. Since the weight of the motor is born by link 1, links 2 and 3 can be 
made more lightweight and the motors themselves can be less powerful. Also 
the dynamics of the parallelogram manipulator are simpler than those of the 
elbow manipulator, thus making it easier to control. 


1.3.2 Spherical Manipulator (RRP) 

By replacing the third or elbow joint in the revolute manipulator by a pris- 
matic joint one obtains the spherical manipulator shown in Figure |1.10 The 
term spherical manipulator derives from the fact that the spherical coor- 
dinates defining the position of the end-effector with respect to a frame whose 
origin lies at the intersection of the three z axes are the same as the first three 
joint variables. Figure |1 .1 1 shows the Stanford Arm, one of the most well- 


12 


INTRODUCTION 


Zo 



known spherical robots. The workspace of a spherical manipulator is shown in 



Fig. 1.11 The Stanford Arm. Photo courtesy of the Coordinated Science Lab, Univer- 
sity of Illinois at Urbana-Champaign. 

Figure |1.12| 


1.3.3 SCARA Manipulator (RRP) 

The SCARA arm (for Selective Compliant Articulated Robot for Assembly) 
shown in Figure |T. 13| is a popular manipulator, which, as its name suggests, is 
tailored for assembly operations. Although the SCARA has an RRP structure, 
it is quite different from the spherical manipulator in both appearance and in its 
range of applications. Unlike the spherical design, which has Zq perpendicular 
to z i, and Z\ perpendicular to Z 2 , the SCARA has Zq,Z\, and z 2 mutually 
parallel. Figure m shows the Epson E2L653S, a manipulator of this type. 
The SCARA manipulator workspace is shown in Figure 1.15 
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Fig. 1.12 Workspace of the spherical manipulator. 
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1.3.4 Cylindrical Manipulator (RPP) 


The cylindrical manipulator is shown in Figure 1.16 The first joint is revolute 


and produces a rotation about the base, while the second and third joints 
are prismatic. As the name suggests, the joint variables are the cylindrical 
coordinates of the end-effector with respect to the base. A cylindrical robot, the 
Seiko RT3300, is shown in Figure [T. 1 7[ with its workspace shown in Figure |T. 18[ 
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Fig. 1.14 Tlie Epson E2L653S SCARA Robot. Photo Courtesy of Epson. 



Fig. 1.15 Workspace of the SCARA manipulator. 


1.3.5 Cartesian manipulator (PPP) 

A manipulator whose first three joints are prismatic is known as a Cartesian 
manipulator, shown in Figure |1.19| 

For the Cartesian manipulator the joint variables are the Cartesian coordi- 
nates of the end-effector with respect to the base. As might be expected the 
kinematic description of this manipulator is the simplest of all manipulators. 
Cartesian manipulators are useful for table-top assembly applications and, as 
gantry robots, for transfer of material or cargo. An example of a Cartesian 
robot, from Epson-Seiko, is shown in Figure [T.20| The workspace of a Carte- 


sian manipulator is shown in Figure 1.21 
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Fig. 1.16 The cylindrical manipulator. 



Fig. 1.17 The Seiko RT3300 Robot. Photo courtesy of Seiko. 


1.3.6 Parallel Manipulator 

A parallel manipulator is one in which some subset of the links form a closed 
chain. More specifically, a parallel manipulator has two or more independent 
kinematic chains connecting the base to the end-effector. Figure |T22 shows the 
ABB IRB 940 Tricept robot, which is a parallel manipulator. The closed chain 
kinematics of parallel robots can result in greater structural rigidity, and hence 
greater accuracy, than open chain robots. The kinematic description of parallel 
robots is fundamentally different from that of serial link robots and therefore 
requires different methods of analysis. 
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Fig. 1.18 Workspace of the cylindrical manipulator. 
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Fig. 1.19 The Cartesian manipulator. 


1.4 OUTLINE OF THE TEXT 


A typical application involving an industrial manipulator is shown in Fig- 
ure 1 1.23 


The manipulator is shown with a grinding tool that it must use 
to remove a certain amount of metal from a surface. In the present text we are 
concerned with the following question: What are the basic issues to be resolved 
and what must we learn in order to be able to program a robot to perform such 
tasks? 

The ability to answer this question for a full six degree-of-freedom manip- 
ulator represents the goal of the present text. The answer is too complicated 
to be presented at this point. We can, however, use the simple two-link planar 
mechanism to illustrate some of the major issues involved and to preview the 
topics covered in this text. 
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Fig. 1.20 The Epson Cartesian Robot. Photo courtesy of Epson. 



Suppose we wish to move the manipulator from its home position to position 
A, from which point the robot is to follow the contour of the surface S to the 
point B, at constant velocity, while maintaining a prescribed force F normal 
to the surface. In so doing the robot will cut or grind the surface according 
to a predetermined specification. To accomplish this and even more general 
tasks, a we must solve a number of problems. Below we give examples of these 
problems, all of which will be treated in more detail in the remainder of the 
text. 


Forward Kinematics 

The first problem encountered is to describe both the position of the tool and 
the locations A and B (and most likely the entire surface S ) with respect to a 
common coordinate system. In Chapter [2] we give some background on repre- 
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Fig. 1.22 The ABB IRB940 Tricept Parallel Robot. Photo courtesy of ABB. 



Fig. 1.23 Two-link planar robot example. 


sentations of coordinate systems and transformations among various coordinate 
systems. 

Typically, the manipulator will be able to sense its own position in some 
manner using internal sensors (position encoders located at joints 1 and 2) that 
can measure directly the joint angles 6 \ and 62 . We also need therefore to 
express the positions A and B in terms of these joint angles. This leads to the 
forward kinematics problem studied in Chapter [3j which is to determine 
the position and orientation of the end-effector or tool in terms of the joint 
variables. 

It is customary to establish a fixed coordinate system, called the world or 
base frame to which all objects including the manipulator are referenced. In 
this case we establish the base coordinate frame ooXoyo at the base of the robot, 
as shown in Figure 1.24 The coordinates (x, y) of the tool are expressed in this 
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Fig. 1.24 Coordinate frames for two-link planar robot. 


coordinate frame as 


x = X 2 = ot\ cos + a 2 cos(0i + 0 2 ) (1.1) 

y = y 2 = an sin 9\ + a 2 sin(0i + d 2 ) (1.2) 

in which a\ and 02 are the lengths of the two links, respectively. Also the 
orientation of the tool frame relative to the base frame is given by the 
direction cosines of the X 2 and y 2 axes relative to the Xq and yo axes, that is, 

x 2 ■ x 0 = cos(0i + 0 2 ); x 2 • yo = - sin(0i + 6 2 ) 

y 2 -x 0 = sin(6>! +6» 2 ); y 2 ■ Vo = cos{9 1 + 0 2 ) 


which we may combine into an orientation matrix 


x 2 ■ x 0 y 2 ■ x 0 


cos(0i + 62) 

-sin(0i + 0 2 ) 

x 2 - yo 2/2 • 2/o 


sin(#i + 0 2 ) 

cos(0i + 8 2 ) 


(1.3) 


Equations (1.1 1 , (1.2 1 and (1.3 1 are called the forward kinematic equa- 
tions for this arm. For a six degree-of-freedom robot these equations are quite 
complex and cannot be written down as easily as for the two-link manipula- 
tor. The general procedure that we discuss in Chapter [3] establishes coordinate 
frames at each joint and allows one to transform systematically among these 
frames using matrix transformations. The procedure that we use is referred to 
as the Denavit-Hartenberg convention. We then use homogeneous coor- 
dinates and homogeneous transformations to simplify the transformation 
among coordinate frames. 


Inverse Kinematics 

Now, given the joint angles 6 1 , d 2 we can determine the end-effector coordinates 
x and y. In order to command the robot to move to location A we need the 
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inverse; that is, we need the joint variables 6 1,62 hr terms of the x and y 
coordinates of A. This is the problem of inverse kinematics. In other words, 
given x and y in the forward kinematic Equations (1.1) and (1.2 1 , we wish to 
solve for the joint angles. Since the forward kinematic equations are nonlinear, 
a solution may not be easy to find, nor is there a unique solution in general. 
We can see in the case of a two-link planar mechanism that there may be no 
solution, for example if the given ( x , y) coordinates are out of reach of the 
manipulator. If the given (x, y) coordinates are within the manipulator’s reach 
there may be two solutions as shown in Figure 1.25 the so-called elbow up 



Fig. 1.25 Multiple inverse kinematic solutions. 


and elbow down configurations, or there may be exactly one solution if the 
manipulator must be fully extended to reach the point. There may even be an 
infinite number of solutions in some cases (Problem I]|25 ) . 

Consider the diagram of Figure |T.26[ Using the Law of Cosines we see that 



Fig. 1.26 Solving for the joint angles of a two- link planar arm. 
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the angle 02 is given by 

cos 62 


x 1 + y 2 — o>i — a 2 
2ct\Oi2 


■= D 


We could now determine 0 2 as 


(1.4) 


O 2 = cos 1 {D) (1.5) 

However, a better way to find 62 is to notice that if cos(02) is given by 


Equation (1.4 1 then sin((? 2 ) is given as 

sin(0 2 ) = ±\/l — D 2 


and, hence, 62 can be found by 


= tan 




D 


( 1 . 6 ) 


(1.7) 


The advantage of this latter approach is that both the elbow-up and elbow- 
down solutions are recovered by choosing the positive and negative signs in 


Equation (1.7 1, respectively. 

It is left as an exercise (Problem |T]|T9| to show that 9\ is now given as 


6 1 = tan ( y/x ) — tan 


-1 


«2 sin 62 
ai + 0-2 cos 9 2 


( 1 . 8 ) 


Notice that the angle 9\ depends on 02 . This makes sense physically since 
we would expect to require a different value for 9\, depending on which solution 
is chosen for 02 - 


Velocity Kinematics 

In order to follow a contour at constant velocity, or at any prescribed velocity, 
we must know the relationship between the velocity of the tool and the joint 
velocities. In this case we can differentiate Equations and (|1.2[) to obtain 


x = — ai sin0! • 0! — a 2 sin(0i + 02 )(^i + ^ 2 ) 

y = ai cos • 9i + 0.2 cos(0i + 02 )[ 0 \ + 82 ) 


(1.9) 


Using the vector notation x = 
equations as 


and 9 = 


02 


we may write these 


x = 


— oq sin 9 i — a 2 sin( 0 i + 0 2 ) —012 sin( 0 i + 0 2 ) 

a\ cos 0 i + 02 cos( 0 i + 02) «2 cos( 0 i + 02) 


0 (1.10) 


= J0 
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The matrix J defined by Equation (1.10) is called the Jacobian of the 


manipulator and is a fundamental object to determine for any manipulator. In 
Chapter [4] we present a systematic procedure for deriving the Jacobian for any 
manipulator in the so-called cross-product form. 

The determination of the joint velocities from the end-effector velocities is 
conceptually simple since the velocity relationship is linear. Thus the joint 
velocities are found from the end-effector velocities via the inverse Jacobian 


= J 


— 1 • 


( 1 . 11 ) 


where J 1 is given by 


J" 1 = 


Ot-\OL 2 .Sg 2 


ot2Ce 1 +e 2 

-CKiCg 1 — a 2 Cg 1+ g 2 


ot2Sg 1 +g 2 

— <T\Sg 1 — a 2 Sg 1+ g 2 


( 1 . 12 ) 


in which eg and sg denote respectively cos 6 and sin0. The determinant of the 


Jacobian in Equation (1.10 1 is a\a 2 Sm.d 2 - The Jacobian does not have an in- 


verse, therefore, when 0 2 = 0 or 7r, in which case the manipulator is said to be 
in a singular configuration, such as shown in Figure |1.27| for 0 2 = 0. The 


Vo 



Fig. 1.27 A singular configuration. 

determination of such singular configurations is important for several reasons. 
At singular configurations there are infinitesimal motions that are unachiev- 
able; that is, the manipulator end-effector cannot move in certain directions. 
In the above cases the end effector cannot move in the positive x 2 direction 
when 6 2 = 0. Singular configurations are also related to the nonuniqueness 
of solutions of the inverse kinematics. For example, for a given end-effector 
position, there are in general two possible solutions to the inverse kinematics. 
Note that a singular configuration separates these two solutions in the sense 
that the manipulator cannot go from one configuration to the other without 
passing through a singularity. For many applications it is important to plan 
manipulator motions in such a way that singular configurations are avoided. 
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Path Planning and Trajectory Generation 

The robot control problem is typically decomposed hierarchically into three 
tasks: path planning, trajectory generation, and trajectory tracking. The path 
planning problem, considered in Chapter [5] is to determine a path in task 
space (or configuration space) to move the robot to a goal position while avoid- 
ing collisions with objects in its workspace. These paths encode position and 
orientation information without timing considerations, i.e. without considering 
velocities and accelerations along the planned paths. The trajectory generation 
problem, also considered in Chapter [5j is to generate reference trajectories that 
determine the time history of the manipulator along a given path or between 
initial and final configurations. 


Dynamics 

A robot manipulator is primarily a positioning device. To control the position 
we must know the dynamic properties of the manipulator in order to know how 
much force to exert on it to cause it to move: too little force and the manipulator 
is slow to react; too much force and the arm may crash into objects or oscillate 
about its desired position. 

Deriving the dynamic equations of motion for robots is not a simple task 
due to the large number of degrees of freedom and nonlinearities present in the 
system. In Chapter [6] we develop techniques based on Lagrangian dynamics for 
systematically deriving the equations of motion of such a system. In addition 
to the rigid links, the complete description of robot dynamics includes the 
dynamics of the actuators that produce the forces and torques to drive the 
robot, and the dynamics of the drive trains that transmit the power from the 
actuators to the links. Thus, in Chapter [7] we also discuss actuator and drive 
train dynamics and their effects on the control problem. 

Position Control 

In Chapters [7] and [8] we discuss the design of control algorithms for the execution 
of programmed tasks. The motion control problem consists of the Tracking 
and Disturbance Rejection Problem, which is the problem of determining 
the control inputs necessary to follow, or track, a desired trajectory that has 
been planned for the manipulator, while simultaneously rejecting disturbances 
due to unmodeled dynamic effects such as friction and noise. We detail the stan- 
dard approaches to robot control based on frequency domain techniques. We 
also introduce the notion of feedforward control and the techniques of com- 
puted torque and inverse dynamics as a means for compensating the com- 
plex nonlinear interaction forces among the links of the manipulator. Robust 
and adaptive control are introduced in Chapter [8] using the Second Method 
of Lyapunov. Chapter [lO] provides some additional advanced techniques from 
nonlinear control theory that are useful for controlling high performance robots. 
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Force Control 

Once the manipulator has reached location A. it must follow the contour S 
maintaining a constant force normal to the surface. Conceivably, knowing the 
location of the object and the shape of the contour, one could carry out this 
task using position control alone. This would be quite difficult to accomplish 
in practice, however. Since the manipulator itself possesses high rigidity, any 
errors in position due to uncertainty in the exact location of the surface or 
tool would give rise to extremely large forces at the end-effector that could 
damage the tool, the surface, or the robot. A better approach is to measure the 
forces of interaction directly and use a force control scheme to accomplish the 
task. In Chapter [9] we discuss force control and compliance along with common 
approaches to force control, namely hybrid control and impedance control. 


Vision 

Cameras have become reliable and relatively inexpensive sensors in many robotic 
applications. Unlike joint sensors, which give information about the internal 
configuration of the robot, cameras can be used not only to measure the position 
of the robot but also to locate objects external to the robot in its workspace. 
In Chapter El we discuss the use of computer vision to determine position and 
orientation of objects. 

Vision-based Control 

In some cases, we may wish to control the motion of the manipulator relative 
to some target as the end-effector moves through free space. Here, force control 
cannot be used. Instead, we can use computer vision to close the control loop 
around the vision sensor. This is the topic of Chapter [12] There are several 
approaches to vision-based control, but we will focus on the method of Image- 
Based Visual Servo (IB VS). This method has become very popular in recent 
years, and it relies on mathematical development analogous to that given in 
Chapter [U 


1.5 CHAPTER SUMMARY 

In this chapter, we have given an introductory overview of some of the basic 
concepts required to develop mathematical models for robot arms. We have 
also discussed a few of the relevant mechanical aspects of robotic systems. In 
the remainder of the text, we will address the basic problems confronted in 
sensor-based robotic manipulation. 

Many books have been written about these and more advance topics, in- 
cluding mm ei hd] urn on un 12m 1221 bd miiraEi! edesi^ii^izb 

mm There is a great deal of ongoing research in robotics. Current research 
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results can be found in journals such as IEEE Transactions on Robotics (pre- 
viously IEEE Transactions on Robotics and Automation ), IEEE Robotics and 
Automation Magazine , International Journal of Robotics Research, Robotics 
and Autonomous Systems, Journal of Robotic Systems, Robotica, Journal of In- 
telligent and Robotic Systems, Autonomous Robots, Advanced Robotics, and 
in proceedings from conferences such as IEEE International Conference on 
Robotics and Automation, IEEE International Conference on Intelligent Robots 
and Systems, Workshop on the Algorithmic Foundations of Robotics, Interna- 
tional Symposium on Experimental Robotics, and International Symposium on 
Robotics Research. 
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Problems 


1-1 What are the key features that distinguish robots from other forms of 
automation such as CNC milling machines? 

1-2 Briefly define each of the following terms: forward kinematics, inverse 
kinematics, trajectory planning, workspace, accuracy, repeatability, reso- 
lution, joint variable, spherical wrist, end effector. 

1-3 What are the main ways to classify robots? 

1-4 Make a list of robotics related magazines and journals carried by the 
university library. 

1-5 Make a list of 10 robot applications. For each application discuss which 
type of manipulator would be best suited; which least suited. Justify your 
choices in each case. 

1-6 List several applications for non-servo robots; for point-to point robots, 
for continuous path robots. 

1-7 List five applications that a continuous path robot could do that a point- 
to-point robot could not do. 

1-8 List five applications where computer vision would be useful in robotics. 

1-9 List five applications where either tactile sensing or force feedback control 
would be useful in robotics. 

1-10 Find out how many industrial robots are currently in operation in the 
United States. How many are in operation in Japan? What country 
ranks third in the number of industrial robots in use? 

1-11 Suppose we could close every factory today and reopen then tomorrow 
fully automated with robots. What would be some of the economic and 
social consequences of such a development? 

1-12 Suppose a law were passed banning all future use of industrial robots. 
What would be some of the economic and social consequences of such an 
act? 

1-13 Discuss possible applications where redundant manipulators would be use- 
ful. 

1-14 Referring to Figure |1.28| suppose that the tip of a single link travels a 
distance d between two points. A linear axis would travel the distance d 


CHAPTER SUMMARY 27 


1 9 



Fig. 1.28 Diagram for Problem 1-15 


1-15 

1-16 

1-17 

1-18 

1-19 

1-20 

1-21 

1-22 

1-23 

1-24 


while a rotational link would travel through an arc length id as shown. 
Using the law of cosines show that the distance d is given by 

d = tsj%\ — cos(d)) 


which is of course less than id. With 10-bit accuracy and i = lm, 9 = 90° 
what is the resolution of the linear link? of the rotational link? 


A single-link revolute arm is shown in Figure 1.28 If the length of the 
link is 50 cm and the arm travels 180? what is the control resolution 
obtained with an 8-bit encoder? 


Repeat Problem 1.15 assuming that the 8-bit encoder is located on the 
motor shaft that is connected to the link through a 50:1 gear reduction. 
Assume perfect gears. 

Why is accuracy generally less than repeatability? 


How could manipulator accuracy be improved using direct endpoint sens- 
ing? What other difficulties might direct endpoint sensing introduce into 
the control problem? 


Derive Equation (1.8). 


For the two-link manipulator of Figure 1.24 suppose «i = 02 = 1. Find 
the coordinates of the tool when d\ = | and 62 = f • 

Find the joint angles d \ , 62 when the tool is located at coordinates (|, \ ) . 


If the joint velocities are constant at 9± = 1, O 2 = 2, what is the velocity 
of the tool? What is the instantaneous tool velocity when 6 \ = 62 = f ? 

Write a computer program to plot the joint angles as a function of time 
given the tool locations and velocities as a function of time in Cartesian 
coordinates. 


Suppose we desire that the tool follow a straight line between the points 
(0,2) and (2,0) at constant speed s. Plot the time history of joint angles. 
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1-25 For the two- link planar manipulator of Figure [T.24| is it possible for there 
to be an infinite number of solutions to the inverse kinematic equations? 
If so, explain how this can occur. 



RIGID MOTIONS AND 
HOMOGENEOUS 
TRANSFORMA TIONS 


A large part of robot kinematics is concerned with the establishment of 
various coordinate systems to represent the positions and orientations of rigid 
objects, and with transformations among these coordinate systems. Indeed, the 
geometry of three-dimensional space and of rigid motions plays a central role in 
all aspects of robotic manipulation. In this chapter we study the operations of 
rotation and translation, and introduce the notion of homogeneous transforma- 
tionsQ Homogeneous transformations combine the operations of rotation and 
translation into a single matrix multiplication, and are used in Chapter [3] to 
derive the so-called forward kinematic equations of rigid manipulators. 

We begin by examining representations of points and vectors in a Euclidean 
space equipped with multiple coordinate frames. Following this, we introduce 
the concept of a rotation matrix to represent relative orientations among co- 
ordinate frames. Then we combine these two concepts to build homogeneous 
transformation matrices, which can be used to simultaneously represent the 
position and orientation of one coordinate frame relative to another. Further- 
more, homogeneous transformation matrices can be used to perform coordinate 
transformations. Such transformations allow us to represent various quantities 
in different coordinate frames, a facility that we will often exploit in subsequent 
chapters. 


1 Since we make extensive use of elementary matrix theory, the reader may wish to review 
Appendix |B| before beginning this chapter. 
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Fig. 2.1 Two coordinate frames, a point p, and two vectors «i and V 2 - 


2.1 REPRESENTING POSITIONS 

Before developing representation schemes for points and vectors, it is instructive 
to distinguish between the two fundamental approaches to geometric reasoning: 
the synthetic approach and the analytic approach. In the former, one reasons 
directly about geometric entities (e.g., points or lines), while in the latter, one 
represents these entities using coordinates or equations, and reasoning is per- 
formed via algebraic manipulations. 

Consider Figure |2TT| This figure shows two coordinate frames that differ in 
orientation by an angle of 45°. Using the synthetic approach, without ever as- 
signing coordinates to points or vectors, one can say that x$ is perpendicular to 
?/o, or that v\ x i >2 defines a vector that is perpendicular to the plane containing 
Vi and i> 2 , in this case pointing out of the page. 

In robotics, one typically uses analytic reasoning, since robot tasks are often 
defined using Cartesian coordinates. Of course, in order to assign coordinates 
it is necessary to specify a coordinate frame. Consider again Figure |2.1| We 
could specify the coordinates of the point p with respect to either frame ooXoyo 
or frame o\X\y\. In the former case, we might assign to p the coordinate vector 
(5, 6) t , and in the latter case (— 2.8,4. 2) T . So that the reference frame will 
always be clear, we will adopt a notation in which a superscript is used to 
denote the reference frame. Thus, we would write 


' 5 ' 

1 

' -2.8 ' 

6 

, P = 

4.2 


Geometrically, a point corresponds to a specific location in space. We stress 
here that p is a geometric entity, a point in space, while both p° and p l are 
coordinate vectors that represent the location of this point in space with respect 
to coordinate frames OoXoyo and oiXij/i, respectively. 
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Since the origin of a coordinate system is just a point in space, we can assign 
coordinates that represent the position of the origin of one coordinate system 
with respect to another. In Figure |2.1[ for example, we have 


' 10 ' 

1 

' -10.6 ' 

5 

> °0 = 

3.5 


In cases where there is only a single coordinate frame, or in which the refer- 
ence frame is obvious, we will often omit the superscript. This is a slight abuse 
of notation, and the reader is advised to bear in mind the difference between the 
geometric entity called p and any particular coordinate vector that is assigned 
to represent p. The former is independent of the choice of coordinate systems, 
while the latter obviously depends on the choice of coordinate frames. 

While a point corresponds to a specific location in space, a vector specifies 
a direction and a magnitude. Vectors can be used, for example, to represent 
displacements or forces. Therefore, while the point p is not equivalent to the 
vector v\, the displacement from the origin oq to the point p is given by the 
vector v\ . In this text, we will use the term vector to refer to what are sometimes 
called free vectors , i.e., vectors that are not constrained to be located at a 
particular point in space. Under this convention, it is clear that points and 
vectors are not equivalent, since points refer to specific locations in space, but 
a vector can be moved to any location in space. Under this convention, two 
vectors are equal if they have the same direction and the same magnitude. 

When assigning coordinates to vectors, we use the same notational conven- 
tion that we used when assigning coordinates to points. Thus, V\ and v-i are 
geometric entities that are invariant with respect to the choice of coordinate 
systems, but the representation by coordinates of these vectors depends directly 
on the choice of reference coordinate frame. In the example of Figure |2.1| we 
would obtain 


' 5 ' 

1 

' 7.77 ' 

o 

' -5.1 ' 

1 

' -2.89 ' 

6 

> = 

0.8 

» V 2 = 

1 

, ^2 = 

4.2 


Coordinate Convention 

In order to perform algebraic manipulations using coordinates, it is essential 
that all coordinate vectors be defined with respect to the same coordinate frame. 
In the case of free vectors, it is enough that they be defined with respect to 
“parallel” coordinate frames, i.e. frames whose respective coordinate axes are 
parallel, since only their magnitude and direction are specified and not their 
absolute locations in space. 


Using this convention, an expression of the form vf + v\, where v\ and v\ 
are as in Figure 2.1 is not defined since the frames OqXqPq and o\X\y\ are not 
parallel. Thus, we see a clear need, not only for a representation system that 
allows points to be expressed with respect to various coordinate systems, but 
also for a mechanism that allows us to transform the coordinates of points that 
are expressed in one coordinate system into the appropriate coordinates with 
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Vo 

V\ 

V 


Xi 



o 0 , Oi 


cos 6 


Fig. 2.2 Coordinate frame oixiyi is oriented at an angle 6 with respect to ooxoyo- 


respect to some other coordinate frame. Such coordinate transformations and 
their derivations are the topic for much of the remainder of this chapter. 


2.2 REPRESENTING ROTATIONS 

In order to represent the relative position and orientation of one rigid body 
with respect to another, we will rigidly attach coordinate frames to each body, 
and then specify the geometric relationships between these coordinate frames. 
In Section |2.1| we saw how one can represent the position of the origin of one 
frame with respect to another frame. In this section, we address the problem 
of describing the orientation of one coordinate frame relative to another frame. 
We begin with the case of rotations in the plane, and then generalize our results 
to the case of orientations in a three dimensional space. 


2.2.1 Rotation in the plane 


Figure 2.2 shows two coordinate frames, with frame oixiyi being obtained by 
rotating frame ooXoyo by an angle 9. Perhaps the most obvious way to represent 
the relative orientation of these two frames is to merely specify the angle of 
rotation, 9. There are two immediate disadvantages to such a representation. 
First, there is a discontinuity in the mapping from relative orientation to the 
value of 9 in a neighborhood of 9 = 0. In particular, for 9 = 2i r— e, small changes 
in orientation can produce large changes in the value of 9 (i.e. , a rotation by e 
causes 9 to “wrap around” to zero). Second, this choice of representation does 
not scale well to the three dimensional case. 

A slightly less obvious way to specify the orientation is to specify the coor- 
dinate vectors for the axes of frame o\X\y\ with respect to coordinate frame 
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Oo^oZ/cQ 

R°i = [4\y°i] 

where x® and yj are the coordinates in frame ooxoyo of unit vectors x\ and 
yi, respectively. A matrix in this form is called a rotation matrix. Rotation 
matrices have a number of special properties that we will discuss below. 

In the two dimensional case, it is straightforward to compute the entries of 
this matrix. As illustrated in Figure [272] 


which gives 


cos 9 

o 

— sin 9 

sin 9 

. 2/1 = 

cos 9 


cos 9 — sin 9 
sin 9 cos 6 


(2.1) 


Note that we have continued to use the notational convention of allowing the 
superscript to denote the reference frame. Thus, Ri is a matrix whose column 
vectors are the coordinates of the (unit vectors along the) axes of frame o\X\yi 
expressed relative to frame ooxoyo . 

Although we have derived the entries for R® in terms of the angle 9, it is not 
necessary that we do so. An alternative approach, and one that scales nicely 
to the three dimensional case, is to build the rotation matrix by projecting the 
axes of frame o\X\y\ onto the coordinate axes of frame oo^oZ/o- Recalling that 
the dot product of two unit vectors gives the projection of one onto the other, 
we obtain 


X\ ■ x 0 

n 

2/i ’ %o 

i 

O 

<c= 

II 

1 

O 

i 


which can be combined to obtain the rotation matrix 


R 


0 

1 


Xi -x 0 yi * Xq 
Xi • 2/o 2/i • Z/o 


Thus the columns of R® specify the direction cosines of the coordinate axes 
of oiXiyi relative to the coordinate axes of oo^oZ/o- For example, the first 
column ( Xi ■ xq, X\ ■ z/o) T of R\ specifies the direction of x\ relative to the frame 
ooXoVo- Note that the right hand sides of these equations are defined in terms 
of geometric entities, and not in terms of their coordinates. Examining Figure 
|2.2|it can be seen that this method of defining the rotation matrix by projection 


gives the same result as was obtained in Equation (2.1). 

If we desired instead to describe the orientation of frame ooXoyo with respect 
to the frame o\X\yi (i.e., if we desired to use the frame 0 \X\yi as the reference 
frame), we would construct a rotation matrix of the form 


R 


i _ 
o — 


x 0 - Xi yo • Xi 
xo ■ 2/i Z/o • 2/i 


2 We will use x t , y- to denote both coordinate axes and unit vectors along the coordinate axes 
depending on the context. 
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Table 2.2.1 


Properties of the Matrix Group SO{n ) 


• R £ SO(n) 

• R- J G SO(n) 

• R- 1 = R t 


• The columns (and therefore the rows) of R are mutually orthogonal 

• Each column (and therefore each row) of R is a unit vector 

• det R = 1 


Since the inner product is commutative, (i.e. Xi ■ yj = yj • Xi), we see that 


Rl = ( R y 


In a geometric sense, the orientation of ooXoyo with respect to the frame 
o\X\yi is the inverse of the orientation of o\X\y\ with respect to the frame 
OoXoyo- Algebraically, using the fact that coordinate axes are always mutually 
orthogonal, it can readily be seen that 

(f?°) T = (I??)" 1 

The column vectors of R ° are of unit length and mutually orthogonal (Prob- 
lem [2][4]) . Such a matrix is said to be orthogonal. It can also be shown 
(Problem [ 2 J 5 ]) that det I?) 1 = ±1. If we restrict ourselves to right-handed coor- 
dinate systems, as defined in Appendix |b| then det I?) 1 = +1 (Problem [2|5| . It 
is customary to refer to the set of all such nxn matrices by the symbol SO(n), 
which denotes the Special Orthogonal group of order n. The properties of 
such matrices are summarized in Table 12.2.11 

To provide further geometric intuition for the notion of the inverse of a rota- 
tion matrix, note that in the two dimensional case, the inverse of the rotation 
matrix corresponding to a rotation by angle 6 can also be easily computed 
simply by constructing the rotation matrix for a rotation by the angle —6: 


cos(-d) — sin(— 9) 
sin(— 6) cos(— 6) 

cos 6 — sin 9 

sin 6 cos 9 



cos 9 

sin 9 


— sin 9 

cos 9 


T 
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2.2.2 Rotations in three dimensions 

The projection technique described above scales nicely to the three dimensional 
case. In three dimensions, each axis of the frame OiXiyiZi is projected onto 
coordinate frame ooXoyoZo ■ The resulting rotation matrix is given by 


R° = 


Xi ■ x 0 
Xi ■ Vo 
Xi ■ z 0 


y 1 ■ x 0 
2/i • 2/o 
2/1 ' -o 


Zi ■ x 0 
zi ■ 2/o 
Zi ■ z 0 


As was the case for rotation matrices in two dimensions, matrices in this 
form are orthogonal, with determinant equal to 1. In this case, 3x3 rotation 
matrices belong to the group SO( 3). The properties listed in Table 2.2.1 also 
apply to rotation matrices in SO( 3). 


Example 2.1 


Zo, Z 1 



Fig. 2.3 Rotation about zo by an angle 6. 


Suppose the frame 0 \Xiy\Zi is rotated through an angle 6 about the z^-axis, 
and it is desired to find the resulting transformation matrix Note that by 
convention the positive sense for the angle 6 is given by the right hand rule; that 
is, a positive rotation by angle 6 about the z-axis would advance a right-hand 
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threaded screw along the positive 2 -cm.|[] From Figure pTj| we see that 

x\ ■ Xq = cos 9, y\ ■ Xo = — sin 9, 

x 1 -y 0 = sin 9, y 1 ■ y 0 = cos 9 


and 


z 0 ■ z i 


= 1 


while all other dot products are zero. Thus the rotation matrix R® has a par- 
ticularly simple form in this case, namely 


R°i = 


cos 9 — sin 9 0 
sin 9 cos 9 0 

0 0 1 


(2.2) 


The Basic Rotation Matrices 


The rotation matrix given in Equation (2.2 1 is called a basic rotation matrix 


(about the 2 -axis). In this case we find it useful to use the more descriptive 
notation R z g instead of to denote the matrix. It is easy to verify that the 
basic rotation matrix R z e has the properties 


0 — 


which together imply 




(Rz, e) 


= R 


2 , 0+0 


= Jib 


(2.3) 

(2.4) 


(2.5) 


Similarly the basic rotation matrices representing rotations about the x and 
y-axes are given as (Problem [2p| 


fiL 


R n, R 


1 

0 


0 


0 

cos 9 


— sin# 

(2.6) 

0 

sin 9 


cos 9 


cos 9 

0 

sin 9 



0 

1 

0 

(2.7) 

- 

sin 9 

0 

cos 9 



which also satisfy properties analogous to Equations ( 2.3 )-( 2.5 I . 

Example 2.2 


3 See also Appendix [b] 
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Consider the frames ooxoyozo Lind o\Xiy\Zi shown in Figure \2~j\ Projecting 
the unit vectors X\,y\,Z\ onto Xo,yo,Zo gives the coordinates of Xi,yi,Z\ in 

the ooXoyoZo frame. We see that the coordinates of x i are ^^,0, -^j , the 

coordinates of y\ are ^^=,0, and the coordinates of Z\ are (0,1, 0) T . The 

rotation matrix R ° specifying the orientation of OiXiyiZi relative to o^x^y^z^ 
has these as its column vectors, that is, 



( 2 . 8 ) 


Fig. 2.4 Defining the relative orientation of two frames. 


O 


2.3 ROTATIONAL TRANSFORMATIONS 


Figure |2.5| shows a rigid object S to which a coordinate frame o\XiyiZi is 
attached. Given the coordinates p 1 of the point p (i.e., given the coordinates of 
p with respect to the frame o\X\y\Z\), we wish to determine the coordinates of 
p relative to a fixed reference frame oqXqPqZq. The coordinates p 1 = ( u,v,w) T 
satisfy the equation 

p = ux\ + vyi + wz\ 

In a similar way, we can obtain an expression for the coordinates p° by project- 
ing the point p onto the coordinate axes of the frame OqXqPqZq, giving 


P 


0 


P-X 0 

p-yo 
p-z 0 
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Fig. 2.5 Coordinate frame attached to a rigid body. 


Combining these two equations we obtain 


P 


o 


(uxi + vyi + wzi ) • x 0 
(ux i + vyi + wzi) ■ y 0 
(ux i + vyi + wzi) ■ z 0 


ux i • xq + vyi ■ xq + wzi 

* 0 

uxi ■ y 0 + vyi ■ y 0 + wzi 

• 2/o 

UX 1 • Zq + vyi ■ Zq + WZi 

• Z 0 _ 

o 

*5 

o 

O 

1 


u 

Xi ■ yo y 1 • 2/0 zi • 2/o 


V 

Xi ■ Zq 2/1 ' -o zi ■ z 0 _ 


w 


But the matrix in this final equation is merely the rotation matrix i?5, which 
leads to 

p° = R° lP l (2.9) 

Thus, the rotation matrix R\ can be used not only to represent the orien- 
tation of coordinate frame oiX\y\Zi with respect to frame ooxoyozo , but also 
to transform the coordinates of a point from one frame to another. If a given 
point is expressed relative to o\XiyiZ\ by coordinates p 1 , then R^p 1 represents 
the same point expressed relative to the frame ooXo2/o~o- 

We can also use rotation matrices to represent rigid motions that correspond 
to pure rotation. Consider Figure [2T6| One corner of the block in Figure [2ii| a) is 
located at the point p a in space. Figure [2T6| )b) shows the same block after it has 
been rotated about z o by the angle n. In Figure [T^ b), the same corner of the 
block is now located at point P b in space. It is possible to derive the coordinates 
for P b given only the coordinates for p a and the rotation matrix that corresponds 
to the rotation about zq. To see how this can be accomplished, imagine that 
a coordinate frame is rigidly attached to the block in Figure [2~fij(a). such that 
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Fig. 2.6 The block in (b) is obtained by rotating the block in (a) by 7r about zo- 


it is coincident with the frame OqXqUqZq. After the rotation by n, the block’s 
coordinate frame, which is rigidly attached to the block, is also rotated by it. 
If we denote this rotated frame by o\Xiy\Zi, we obtain 


Ri = K, = 


-1 oo 
0-10 
0 0 1 


In the local coordinate frame 0 iX\yiZi , the point pb has the coordinate rep- 
resentation pi . To obtain its coordinates with respect to frame ooXoyoZo, we 
merely apply the coordinate transformation Equation (2.9 1, giving 


Pb — Rz,TvPb 


The key thing to notice is that the local coordinates, pi, of the corner of the 
block do not change as the block rotates, since they are defined in terms of 
the block’s own coordinate frame. Therefore, when the block’s frame is aligned 
with the reference frame OoXoyoZo (he., before the rotation is performed), the 
coordinates pi = since before the rotation is performed, the point p a is 
coincident with the corner of the block. Therefore, we can substitute p° a into 
the previous equation to obtain 


Pb = Rz.kP 


o 

a 


This equation shows us how to use a rotation matrix to represent a rotational 
motion. In particular, if the point pb is obtained by rotating the point p a as 
defined by the rotation matrix R, then the coordinates of pb with respect to 
the reference frame are given by 


Pb = RP° 
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Fig. 2. 7 Rotating a vector about axis yo ■ 


This same approach can be used to rotate vectors with respect to a coordinate 
frame, as the following example illustrates. 


Example 2.3 

The vector v with coordinates v° = (0,1, 1) T is rotated about yo by ^ as 
shown in Figure \2F\ The resulting vector Vi has coordinates given by 


'001' 


' 0 ' 


' 1 ' 

0 10 


1 

= 

1 

-10 0 


1 


1 

o 

1 


(2.10) 

( 2 . 11 ) 


Thus, as we have now seen, a third interpretation of a rotation matrix R is 
as an operator acting on vectors in a fixed frame. In other words, instead of 
relating the coordinates of a fixed vector with respect to two different coordinate 
frames, Equation (2.101 can represent the coordinates in ooXoyoZo of a vector 
v\ that is obtained from a vector v by a given rotation. 


Summary 

We have seen that a rotation matrix, either R £ SO( 3) or R £ SO( 2), can be 
interpreted in three distinct ways: 


1. It represents a coordinate transformation relating the coordinates of a 
point p in two different frames. 

2. It gives the orientation of a transformed coordinate frame with respect to 
a fixed coordinate frame. 

3. It is an operator taking a vector and rotating it to a new vector in the 
same coordinate system. 
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The particular interpretation of a given rotation matrix R that is being used 
must then be made clear by the context. 

2.3.1 Similarity Transformations 

A coordinate frame is defined by a set of basis vectors, for example, unit vec- 
tors along the three coordinate axes. This means that a rotation matrix, as a 
coordinate transformation, can also be viewed as defining a change of basis from 
one frame to another. The matrix representation of a general linear transfor- 
mation is transformed from one frame to another using a so-called similarity 
transformatiorj^] For example, if A is the matrix representation of a given 
linear transformation in ooxoyozo and B is the representation of the same linear 
transformation in o\XiyiZi then A and B are related as 

B = (R^AR^ (2.12) 

where 1?° is the coordinate transformation between frames 0 \X\yiZ\ and oqXqPqZq 
In particular, if A itself is a rotation, then so is B, and thus the use of similarity 
transformations allows us to express the same rotation easily with respect to 
different frames. 

Example 2.4 

Henceforth, whenever convenient we use the shorthand notation eg = cos 0, 
sg = sin0 for trigonometric functions. Suppose frames ooxoyozo and o\Xiy\Zi 
are related by the rotation 


R 


0 

1 


0 0 1 
0 10 
-10 0 


as shown in Figure 2 .f 


relative to frame o\Xiy\Zi 


If A = R Zi g 
we have 


relative to the frame ooxoyozo , then, 


B = {R° 1 )- 1 A°R° 1 


1 0 0 
0 Cg Sg 
0 ~sg Cg 


In other words, B is a rotation about the z^-axis but expressed relative to the 
frame o\X\y\Z\. This notion will be useful below and in later sections, 
o 


4 See Appendix [b] 
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Fig. 2.8 Coordinate Frames for Example |2.4| 


2.4 COMPOSITION OF ROTATIONS 

In this section we discuss the composition of rotations. It is important for 
subsequent chapters that the reader understand the material in this section 
thoroughly before moving on. 


2.4.1 Rotation with respect to the current frame 


Recall that the matrix R® in Equation (2.9 1 represents a rotational transfor- 


mation between the frames ooXoyo z o and o\X\y\Z\. Suppose we now add a 
third coordinate frame 0212^222 related to the frames oqXqPqZq and o\Xiy\Zi 
by rotational transformations. A given point p can then be represented by co- 
ordinates specified with respect to any of these three frames: p° , p 1 and p 2 . The 
relationship among these representations of p is 


p° 

= R°iP l 

(2.13) 

p 1 

= R\p 2 

(2.14) 

p° 

= RV 

(2.15) 


where each R 1 :- is a rotation matrix. Substituting Equation (2.14) into Equation 


(2.131 results in 


p° = R°R\p 2 


(2.16) 


Note that R\ and represent rotations relative to the frame oqXqPqZq while 
R\ represents a rotation relative to the frame OiXiyiZ-y. Comparing Equa- 


tions (2.151 and (2.161 we can immediately infer 


TD 0 r>0 TD 1 

il 2 — ^1^2 


(2.17) 


Equation (2.171 is the composition law for rotational transformations. It states 


that, in order to transform the coordinates of a point p from its representation 
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Fig. 2.9 Composition of rotations about current axes. 


p 2 in the frame 02X29222 to its representation p° in the frame OqXo9oZo, we may 
first transform to its coordinates p 1 in the frame 0 \X\y\Z\ using R\ and then 
transform p 1 to p° using 


We may also interpret Equation (2.17) as follows. Suppose initially that all 
three of the coordinate frames coincide. We first rotate the frame 02X29222 
relative to Oo£o2/o 2 o according to the transformation R®. Then, with the frames 
0\XiyiZi and 02X29222 coincident, we rotate 02X29222 relative to oiX\9\Z\ ac- 
cording to the transformation R\. In each case we call the frame relative to 
which the rotation occurs the current frame. 


Example 2.5 

Suppose a rotation matrix R represents a rotation of angle <j> about the current 
9-axis followed by a rotation of angle 9 about the current z-axis. Refer to 
Figure 2. 9 Then the matrix R is given by 


^ X^y ,(px^z,0 


C-(j) 

0 

S(f) 


ce 

-s$ 

0 ' 

0 

1 

0 


SB 

CB 

0 


0 

C(j) 


0 

0 

1 


C<j}C$ C(f>SQ S(f) 
S 8 CB 0 

S(pCQ S(j)SQ C(p 


(2.18) 


o 

It is important to remember that the order in which a sequence of rotations 
are carried out, and consequently the order in which the rotation matrices are 
multiplied together, is crucial. The reason is that rotation, unlike position, 
is not a vector quantity and so rotational transformations do not commute in 
general. 

Example 2.6 

Suppose that the above rotations are performed in the reverse order, that is, 
first a rotation about the current z-axis followed by a rotation about the current 
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y-axis. Then the resulting rotation matrix is given by 


R — Rz,gRy,4> 


eg 

S(f) 

o ' 


C(p 

0 

S0 

sg 

Co 

0 


0 

1 

0 

0 

0 

l 


~S ( j > 

0 

C(j) 


CgC<p — Sg CgSff, 

$QC(p Cq SqS(J) 

S(f> 0 C(p 


Comparing Equations (2.18) and (2.19) we see that R ^ R' 


(2.19) 


2.4.2 Rotation with respect to the fixed frame 


Many times it is desired to perform a sequence of rotations, each about a 
given fixed coordinate frame, rather than about successive current frames. For 
example we may wish to perform a rotation about Xq followed by a rotation 
about j/o (and not y\\). We will refer to ooXoyo z o as the fixed frame. In this 
case the composition law given by Equation (2.171 is not valid. It turns out 


that the correct composition law in this case is simply to multiply the successive 


rotation matrices in the reverse order from that given by Equation (2.17). Note 


that the rotations themselves are not performed in reverse order. Rather they 
are performed about the fixed frame instead of about the current frame. 

To see why this is so, suppose we have two frames OqXqPqZq and 0 iXiy\Z\ 
related by the rotational transformation R®. If R £ 50(3) represents a rotation 
relative to ooXoyozo we know from Section HAH that the representation for R 
in the current frame 0\X\y\Z\ is given by (i?) ) ) _1 i?i?°. Therefore, applying the 
composition law for rotations about the current axis yields 


R 


o 

2 


R° [(i?5) _1 i?i?°] = RRi 


( 2 . 20 ) 
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Fig. 2.10 Composition of rotations about fixed axes. 


Example 2.7 

Referring to Figure 2.10 suppose that a rotation matrix R represents a ro- 
tation of angle <j> about yo followed by a rotation of angle 0 about the fixed zq. 

The second rotation about the fixed axis is given by R y _ ( j ) R z g R y which 
is the basic rotation about the z-axis expressed relative to the frame 0 \XiyiZ\ 
using a similarity transformation. Therefore, the composition rule for rotational 
transformations gives us 


P° = Ry^P 1 

= Ry,<t> [Ry,-<pRz,eRy,(l>] p 2 (2-21) 

= Rz^Ry^p 2 


It is not necessary to remember the above derivation, only to note by comparing 
Equation (2.21) with Equation (2.18) that we obtain the same basic rotation 
matrices, but in the reverse order. 


o 
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Summary 

We can summarize the rule of composition of rotational transformations by 
the following recipe. Given a fixed frame OqXqPqZq a current frame 0\X\y\Z \ , 
together with rotation matrix R® relating them, if a third frame 02X2112Z2 is 
obtained by a rotation R performed relative to the current frame then post- 
multiply Rf by II = R\ to obtain 

R% = RORl ( 2 . 22 ) 

If the second rotation is to be performed relative to the fixed frame then 
it is both confusing and inappropriate to use the notation R\ to represent this 
rotation. Therefore, if we represent the rotation by R , we premultiply Rf by 
R to obtain 


pU p pU 

-Ll 2 — Jlit^ 


( 2 . 23 ) 


In each case f?!] represents the transformation between the frames oqXqDqZq and 
02X2V2Z2- The frame O2X2IJ2Z2 that results in Equation ( 2 . 22 ) will be different 
from that resulting from Equation ( 2 . 231 . 

Using the above rule for composition of rotations, it is an easy matter to 
determine the result of multiple sequential rotational transformations. 

Example 2.8 

Suppose R is defined by the following sequence of basic rotations in the order 
specified: 

1. A rotation of 9 about the current x-axis 

2. A rotation of <j> about the current z-axis 

3. A rotation of a about the fixed z-axis 

4- A rotation of (3 about the current y-axis 

5. A rotation of 5 about the fixed x-axis 

In order to determine the cumulative effect of these rotations we simply begin 
with the first rotation R Xi e and pre- or post-multiply as the case may be to obtain 


dl — R x ,8Rz,aRx,oRz,<f>Ry,f3 


( 2 . 24 ) 


o 


2.5 PARAMETERIZATIONS OF ROTATIONS 

The nine elements r. c j in a general rotational transformation R are not inde- 
pendent quantities. Indeed a rigid body possesses at most three rotational 
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Fig. 2.11 Euler angle representation. 


degrees-of-freedom and thus at most three quantities are required to specify its 
orientation. This can be easily seen by examining the constraints that govern 
the matrices in SO( 3): 


H4 

i 

rurij + r 2i r 2 j + r 3i r 3j 


1, j€ {1,2,3} 

0 , i^j 


(2.25) 

(2.26) 


Equation (2.25) follows from the fact the the columns of a rotation matrix 
are unit vectors, and Equation (2.261 follows from the fact that columns of a 
rotation matrix are mutually orthogonal. Together, these constraints define six 
independent equations with nine unknowns, which implies that there are three 
free variables. 

In this section we derive three ways in which an arbitrary rotation can be 
represented using only three independent quantities: the Euler Angle repre- 
sentation, the roll-pitch- yaw representation, and the axis/angle representa- 
tion. 


2.5.1 Euler Angles 


A common method of specifying a rotation matrix in terms of three independent 
quantities is to use the so-called Euler Angles. Consider the fixed coordinate 
frame ooxoyozo and the rotated frame o 3 X\y\Zi shown in Figure 2.11 We can 


specify the orientation of the frame o\Xiy\Zi relative to the frame OoXoyoZo by 
three angles (cf>,9,i/j), known as Euler Angles, and obtained by three successive 
rotations as follows: First rotate about the z-axis by the angle <f>. Next rotate 
about the current y - axis by the angle 9. Finally rotate about the current z-axis 
by the angle ip. In Figure 2.11[ frame o a x a y a z a represents the new coordinate 
frame after the rotation by cp , frame ObXbybZb represents the new coordinate 
frame after the rotation by 9 , and frame 0 \X 3 yiZi represents the final frame, 
after the rotation by ip. Frames o a x a yaZ a and ObXbybZb are shown in the figure 
only to help you visualize the rotations. 
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In terms of the basic rotation matrices the resulting rotational transformation 
Ri can be generated as the product 


Rzyz — 


C(j) S(p 0 

Sfi C</> 0 

0 0 1 


C(j)CQCip SfjySqp C^CqS^ S(pCip C(j)SQ 

+ CfiS'il) S^CQSnjj + C^C^p S^Sq 

SQC'ijj SQS^ Cq 



eg 0 Sg 


1 

0 

co 

1 

O 

1 


0 10 


•s ? p 0 


— Sg 0 eg 


° 0 1 _ 


(2.27) 


The matrix Rzyz in Equation (2.271 is called the ZY Z-F,uler Angle Trans- 
formation. 

The more important and more difficult problem is the following: Given a 
matrix R S SO( 3) 


R = 


r 11 

r 12 

T 13 

T 21 

T 22 

T23 

T31 

T 32 

T 33 

9 , and 7 p 

so that 


R = Rzyz 


(2.28) 


This problem will be important later when we address the inverse kinematics 
problem for manipulators. In order to find a solution for this problem we break 
it down into two cases. 


First, suppose that not both of ri 3 , r 23 are zero. Then from Equation (2.28 1 


we deduce that sg ^ 0, and hence that not both of 7 - 31 , r 32 are zero. If not both 
ri 3 and 7-23 are zero, then 7-33 ^ ± 1 , and we have eg = r 3 3 , sg = ± y/l — r ■ 


33 


so 


atan2 ( r 33 , 


1 - r 3.3 


or 


(2.29) 


(2.30) 


where the function atan2 is the two-argument arctangent function defined 
in Appendix A. 


0 = 


9 = atan2 ( r 33 , — \/l — r| 3 


If we choose the value for 0 given by Equation (2.29), then sg > 0, and 

<t> = atan2(ri3,r 23 ) (2.31) 

7(1 = atan2(— r 3 i,r 32 ) (2.32) 

If we choose the value for 8 given by Equation ( 2.30| ), then sg < 0, and 

4> = atan2(— ri 3 , — r 23 ) (2.33) 

7(1 = atan2(r 3 i, — r 32 ) (2-34) 
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Thus there are two solutions depending on the sign chosen for 9. 

If r 13 = r 23 = 0, then the fact that R is orthogonal implies that r 33 = ±1, 
and that r 3 i = r 3 2 = 0. Thus R has the form 


R 


r n r i2 0 

r2i r 2 2 0 

0 0 ±1 


(2.35) 


If r 33 = 1, then eg 
becomes 


1 and sg = 0, so that 9 = 0. 


In this case Equation 


(2.271 


C0C-0 S<f>S^ 

CfpSijj S(j)Cip 

0 ■ 




o " 

+ C(j,Sijj 

S05-0 + CfjyCijj 

0 

= 

S(f)-\-Tp 


0 

0 

0 

1 


0 

0 

l 


Thus the sum <p + ip can be determined as 


<p + ip = atan2(rn, r 2 i) 


(2.36) 


= atan2(rn, — r 32 ) 


Since only the sum <p + ip can be determined in this case there are infinitely 
many solutions. In this case, we may take (p = 0 by convention. If r 33 = —1, 


then eg = — 1 and sg = 0, so that 9 = tt. In this case Equation (2.271 becomes 


C0 — -0 


0 ' 


rn 

r\2 

0 ' 


Ccp-Tp 

0 

= 

1~21 

T22 

0 

0 

0 

-1 


0 

0 

-1 


The solution is thus 

cp — ip = atan2(— m, — r 32 ) 
As before there are infinitely many solutions. 


(2.37) 


(2.38) 


2.5.2 Roll, Pitch, Yaw Angles 

A rotation matrix R can also be described as a product of successive rotations 
about the principal coordinate axes xo,yo , and zo taken in a specific order. 
These rotations define the roll, pitch, and yaw angles, which we shall also 
denote <p, 9, ip, and which are shown in Figure |2.12| 

We specify the order of rotation as x — y ~ z, in other words, first a yaw 
about Xq through an angle ip, then pitch about the yo by an angle 9, and finally 
roll about the zq by an angle Since the successive rotations are relative to 


5 It should be noted that other conventions exist for naming the roll, pitch and yaw angles. 
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Fig. 2.12 Roll, pitch, and yaw angles. 


the fixed frame, the resulting transformation matrix is given by 
RxY Z ki. z . o 0 ^ 


C(j) 

S(f> 

o 


-S0 0 

c<j, 0 

0 1 



Cg 0 S 8 


o 

O 

T— 1 


0 10 


0 s^p 


<35 

0 

o 

<35 

CO 

1 


0 Cip 


C<pC6 SpC-p “T CpSQS^p 

S(f)CQ T S^SqStJj 

$6 eg Sip 


ScpSi/j T e^SgCip 
C(j)Sip T SpSgCip 

ceCif, 


(2.39) 


Of course, instead of yaw-pitch-roll relative to the fixed frames we could also 
interpret the above transformation as roll-pitch-yaw, in that order, each taken 
with respect to the current frame. The end result is the same matrix as in 


Equation ( |2.39[ ). 

The three angles, </>, 0, ip, can be obtained for a given rotation matrix using a 
method that is similar to that used to derive the Euler angles above. We leave 
this as an exercise for the reader. 


2.5.3 Axis/Angle Representation 

Rotations are not always performed about the principal coordinate axes. We 
are often interested in a rotation about an arbitrary axis in space. This pro- 
vides both a convenient way to describe rotations, and an alternative parame- 
terization for rotation matrices. Let k = (k x , k y , k z ) T , expressed in the frame 
oox 0 y 0 z 0 , be a unit vector defining an axis. We wish to derive the rotation 
matrix R ^ & representing a rotation of 6 about this axis. 

There are several ways in which the matrix R , can be derived. Perhaps the 
simplest way is to note that the axis define by the vector k is along the 2 -axis 
following the rotational transformation R® = R z a R y p. Therefore, a rotation 
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about the axis k can be computed using a similarity transformation as 


R k,e 


tdO td tdO 1 
Ix l lx z,6 ll l 


(2.40) 

(2.41) 



Fig. 2.13 Rotation about an arbitrary axis. 


From Figure 2.13 


we see that 


sin a = 

k y 

(2.42) 

yj k l + k l 

cos a = 

fox 

(2.43) 

\jk% + ky 

sin/3 = 

\J k l + k l 

(2.44) 

cos/3 = 

k z 

(2.45) 


Note that the final two equations follow from the fact that k is a unit vector. 


lengthy calculation (Problem 2 


Substituting Equations ( 2.42 )-( 2.45) into Equation (2.411 we obtain after some 


17) 


klvg + Cg 

kxkyVg k z Sg 

kx kz^o “l - kyS0 

k x k y vg + k z Sg 

k^g + Cg 

k y k z VQ k x se 

k x k z Vg — kySg 

kyk z Vg + k x Sg 

k 2 z v e + c e 


(2.46) 


where vg = vers 0 = 1 — eg. 

In fact, any rotation matrix R £ S'0(3) can be represented by a single rotation 
about a suitable axis in space by a suitable angle, 


“ R kfi 


R 


(2.47) 
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where k is a unit vector defining the axis of rotation, and 9 is the angle of rota- 
tion about k. The matrix R , given in Equation (2.47) is called the axis/angle 
representation of R. Given an arbitrary rotation matrix R with components 
7\j , the equivalent angle 9 and equivalent axis k are given by the expressions 


9 = 


cos 


= cos 


i ( Tr{R) - 1 
( ri i + r 2 2 + r 33 - 1 


(2.48) 


where Tr denotes the trace of R, and 


k = 


1 


r 3 2 - r 2 3 
ri 3 - r 3 1 
r2i - r 12 


(2.49) 


2sin0 

These equations can be obtained by direct manipulation of the entries of the 


matrix given in Equation (2.46 1 . The axis/angle representation is not unique 


since a rotation of — 9 about —k is the same as a rotation of 9 about fc, that is, 

R k,e = R -k-e ( 2 ’ 5 °) 

If 9 = 0 then R is the identity matrix and the axis of rotation is undefined. 


Example 2.9 

Suppose R is generated by a rotation of 90° about zq followed by a rotation 
of 30° about yo followed by a rotation of 60° about Xq. Then 


R 


™x,6oRy,3oRz,90 



V3 1 

2 2 

V3 3 

4 4 

1 V3 

4 4 . 


(2.51) 


We see that Tr(R) = 0 and hence the equivalent angle is given by Equation 
(2.48) as 



The equivalent axis is given from Equation 

11 11 1 \ 

V3’ 2V3 2’2V3 + 2 ) 



2.49 ) 


(2.52) 


(2.53) 


The above axis/angle representation characterizes a given rotation by four 
quantities, namely the three components of the equivalent axis k and the equiv- 
alent angle 9. However, since the equivalent axis k is given as a unit vector only 
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TO APPEAR 


Fig. 2.14 Homogeneous transformations in two dimensions. 


two of its components are independent. The third is constrained by the condi- 
tion that k is of unit length. Therefore, only three independent quantities are 
required in this representation of a rotation R. We can represent the equivalent 
axis/angle by a single vector r as 

r = (r x ,r y ,r z ) T = (9k x ,9k y ,9k z ) T (2-54) 


Note, since k is a unit vector, that the length of the vector r is the equivalent 
angle 9 and the direction of r is the equivalent axis k. 


Remark 2.1 One should be careful not to interpret the representation in Equa- 
tion (2.54) mean that two axis/angle representations may be combined using 
standard rules of vector algebra as doing so would imply that rotations commute 
which, as we have seen, in not true in general. 


2.6 RIGID MOTIONS 

We have seen how to represent both positions and orientations. We combine 
these two concepts in this section to define a rigid motion and, in the next 
section, we derive an efficient matrix representation for rigid motions using the 
notion of homogeneous transformation. 

Definition 2.1 A rigid motion is an ordered pair ( d,R ) where d £ R 3 and 
R € SO(3). The group of all rigid motions is known as the Special Euclidean 
Group and is denoted by SE( 3). We see then that SE(3) = K 3 x SO(3) [^] 


a The definition of rigid motion is sometimes broadened to include reflections, which cor- 
respond to det R = —1. We will always assume in this text that det R = +1, i.e. that 
R e 50(3). 

A rigid motion is a pure translation together with a pure rotation. Referring 
to Figure [2. 14| we see that if frame o\Xiy\Zi is obtained from frame oqXqPqZq by 
first applying a rotation specified by R\ followed by a translation given (with 


54 RIGID MOTIONS AND HOMOGENEOUS TRANSFORMATIONS 


respect to ooXoyoZo) by d”, then the coordinates p° are given by 


p° = ROp 1 + rfO 


(2.55) 


Two points are worth noting in this figure. First, note that we cannot simply 
add the vectors p° and p 1 since they are defined relative to frames with different 
orientations, i.e. with respect to frames that are not parallel. However, we are 
able to add the vectors p 1 and R^p 1 precisely because multiplying p 1 by the 
orientation matrix R <: l expresses p 1 in a frame that is parallel to frame ooa;oyo~o- 
Second, it is not important in which order the rotation and translation are 
performed. 

If we have the two rigid motions 


and 


Rip 1 + d? 

(2.56) 

R\p 2 + d\ 

(2.57) 


then their composition defines a third rigid motion, which we can describe by 
substituting the expression for p 1 from Equation (2.571 into Equation (2.561 


p° = R\R\p 2 + R\d\ + d° 


(2.58) 


Since the relationship between p° and p 2 is also a rigid motion, we can equally 
describe it as 


p° = R° 2 p 2 + d° 2 


Comparing Equations (2.58) and (2.591 we have the relationships 


pO pO pi 

rt 2 — iti ri2 

d° 2 = d? + Rid 2 


(2.59) 


(2.60) 

(2.61) 


Equation (2.601 shows that the orientation transformations can simply be mul- 
tiplied together and Equation (2.611 shows that the vector from the origin oq 
to the origin 02 has coordinates given by the sum of d 5 (the vector from 00 
to 01 expressed with respect to oo*o2/o-o) and Rid 2 (the vector from o\ to 02 , 
expressed in the orientation of the coordinate system oo^oyo^o)- 


2.7 HOMOGENEOUS TRANSFORMATIONS 


One can easily see that the calculation leading to Equation (2.58 1 would quickly 
become intractable if a long sequence of rigid motions were considered. In this 
section we show how rigid motions can be represented in matrix form so that 
composition of rigid motions can be reduced to matrix multiplication as was 
the case for composition of rotations. 
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In fact, a comparison of Equations (2.60 ) and ( 2.61 ) with the matrix identity 

d? 


■ f?° 

d? 1 

' R\ 

d? ' 


0 

1 

0 

1 



0 


Rjd'i 


1 


(2.62) 


where 0 denotes the row vector (0,0,0), shows that the rigid motions can be 
represented by the set of matrices of the form 


H 


R d 
0 1 


; R £ 50(3), d£ R 3 


(2.63) 


Transformation matrices of the form given in Equation (2.631 are called ho- 
mogeneous transformations. A homogeneous transformation is therefore 
nothing more than a matrix representation of a rigid motion and we will use 
SE( 3) interchangeably to represent both the set of rigid motions and the set of 
all 4 x 4 matrices H of the form given in Equation (2.631 


Using the fact that R is orthogonal it is an easy exercise to show that the inverse 
transformation H~ l is given by 


H - 1 


R t —R T d ' 
0 1 


(2.64) 


In order to represent the transformation given in Equation (2.55 1 by a matrix 
multiplication, we must augment the vectors p° and p 1 by the addition of a 
fourth component of 1 as follows, 


p o 
P 1 




(2.65) 

( 2 . 66 ) 


The vectors P° and P l are known as homogeneous representations of the 
vectors p° and p 1 , respectively. It can now be seen directly that the trans- 
formation given in Equation (2.551 is equivalent to the (homogeneous) matrix 
equation 


P° = H^P 1 (2.67) 

A set of basic homogeneous transformations generating SE( 3) is given 
by 


x,a 


' 1 

0 

0 

a 


' 1 

0 

0 

o ' 

0 

1 

0 

0 

; R.ot r „ = 

0 

c a 

Sa 

0 

0 

0 

1 

0 

1 ,ct 

0 

Set 

C-a 

0 

0 

0 

0 

1 


0 

0 

0 

1 


Trans. 


( 2 . 68 ) 
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Trans 


Trans 


' 1 

0 

0 

0 ■ 


C/3 

0 

S/3 

0 ' 

0 

1 

0 

b 

to 

O 

II 

0 

1 

0 

0 

0 

0 

1 

0 

-S/3 

0 

C/3 

0 

0 

0 

0 

1 


0 

0 

0 

1 


' 1 

0 

0 

0 ■ 


c 7 

— s 7 

0 

0 ' 

0 

1 

0 

0 

; Rot x ~ = 

S-y 

Cry 

0 

0 

0 

0 

1 

c 


0 

0 

1 

0 

0 

0 

0 

1 


0 

0 

0 

1 


(2.69) 


(2.70) 


for translation and rotation about the x, y , z-axes, respectively. 

The most general homogeneous transformation that we will consider may be 
written now as 


Hi = 


Tly S 'y d"y ^y 

n z s x a z d z 

0 0 0 1 


n s a d 
0 0 0 1 


(2.71) 


In the above equation n = (n x ,n y , n z ) T is a vector representing the direction of 
X\ in the ooxoyozo system, s = (s x , s y , s z ) T represents the direction of y±, and 
a = (a x ,a y ,a z ) T represents the direction of Z\. The vector d = (d x ,d y ,d z ) T 
represents the vector from the origin o o to the origin cq expressed in the frame 
ooXoyoZQ. The rationale behind the choice of letters n, s and a is explained in 
Chapter [3] 


Composition Rule for Homogeneous Transformations 

The same interpretation regarding composition and ordering of transformations 
holds for 4 x 4 homogeneous transformations as for 3x3 rotations. Given a 
homogeneous transformation H ° relating two frames, if a second rigid motion, 
represented by H G SE( 3) is performed relative to the current frame, then 

Hi = H°H 

whereas if the second rigid motion is performed relative to the fixed frame, then 

Hi = II Hi 


Example 2.10 

The homogeneous transformation matrix H that represents a rotation by 
angle a about the current x-axis followed by a translation of b units along the 
current x-axis, followed by a translation of d units along the current z-axis, 
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followed by a rotation by angle 9 about the current z-axis, is given by 


H = Rot x>ol Trans x ^ 

Trans z 

dRot z fi 


Co 

-so 

0 

b ' 


Ccx.Sq 

Col CO 

Sq, 

— ds a 


SolSQ 

s a co 

c a 

dc a 


0 

0 

0 

1 


The homogeneous representation given in Equation (2.63 1 is a special case of 


homogeneous coordinates, which have been extensively used in the field of com- 
puter graphics. There, one is interested in scaling and/or perspective transfor- 
mations in addition to translation and rotation. The most general homogeneous 
transformation takes the form 


H = 


R 


3x3 


<^3xl 


/ 1x3 | s lxl 


Rotation I Translation 


perspective I scale factor 


(2.72) 


For our purposes we always take the last row vector of H to be (0,0, 0,1), 
although the more general form given by (2.721 could be useful, for example, 


for interfacing a vision system into the overall robotic system or for graphic 
simulation. 


2.8 CHAPTER SUMMARY 

In this chapter, we have seen how matrices in SE(n) can be used to represent 
the relative position and orientation of two coordinate frames for n = 2, 3. We 
have adopted a notional convention in which a superscript is used to indicate a 
reference frame. Thus, the notation p° represents the coordinates of the point 
p relative to frame 0. 

The relative orientation of two coordinate frames can be specified by a rota- 
tion matrix, R £ SO{n ), with n = 2,3. In two dimensions, the orientation of 
frame 1 with respect to frame 0 is given by 

cos 6 — sin 9 
sin 9 cos 9 

in which 9 is the angle between the two coordinate frames. In the three dimen- 
sional case, the rotation matrix is given by 

Xi -x 0 2/1 • x 0 zi • x 0 

xi • 2/o 2/i • 2/o • 2/o 

xi ■ z 0 2/i ' zo zi ■ z 0 

In each case, the columns of the rotation matrix are obtained by projecting an 
axis of the target frame (in this case, frame 1) onto the coordinate axes of the 
reference frame (in this case, frame 0). 



Ri = 


Xi ■ x 0 2/1 • Xo 
xi -2/o 2/i • 2/o 


58 RIGID MOTIONS AND HOMOGENEOUS TRANSFORMATIONS 


The set ofnxn rotation matrices is known as the special orthogonal group 
of order n, and is denoted by SO{n). An important property of these matrices 
is that R~ 1 = R t for any R € SO(n). 

Rotation matrices can be used to perform coordinate transformations be- 
tween frames that differ only in orientation. We derived rules for the composi- 
tion of rotational transformations as 

TD 0 TD 0 TD 

it 2 — .ZXi il 

for the case where the second transformation, f?, is performed relative to the 
current frame and 

TD 0 TD TDO 

ii2 — iiitj^ 

for the case where the second transformation, R, is performed relative to the 
fixed frame. 

In the three dimensional case, a rotation matrix can be parameterized using 
three angles. A common convention is to use the Euler angles which 

correspond to successive rotations about the z, y and z axes. The corresponding 
rotation matrix is given by 

Roll, pitch and yaw angles are similar, except that the successive rotations are 
performed with respect to the fixed, world frame instead of being performed 
with respect to the current frame. 

Homogeneous transformations combine rotation and translation. In the three 
dimensional case, a homogeneous transformation has the form 


The set of all such matrices comprises the set SE(3), and these matrices can 
be used to perform coordinate transformations, analogous to rotational trans- 
formations using rotation matrices. 

The interested reader can find deeper explanations of these concepts in a 
variety of sources, including jl] [T5] [25] [52] [53] [75] , 


; R £ 50(3), dsK 3 
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1. Using the fact that Vi ■ v 2 = vfv 2 , show that the dot product of two free 
vectors does not depend on the choice of frames in which their coordinates 
are defined. 


2 . 


Show that the length of a free vector is not changed by rotation, i.e., that 
\\v\\ = \\Rv\\. 


3. Show that the distance between points is not changed by rotation i.e., 
that ||pi -p 2 1 | = \\Rpi - RP2\ |. 

4. If a matrix R satisfies R T R = /, show that the column vectors of R are 
of unit length and mutually perpendicular. 


5. If a matrix R satisfies R T R = /, then 

a) show that det R = ±1 

b) Show that det I? = ±1 if we restrict ourselves to right-handed coordi- 
nate systems. 


6 . 

7. 


Verify Equations ( 2.3 1-( 2.5 ) . 


A group is a set X together with an operation * defined on that set such 
that 


• X\ * x 2 £ X for all x±, X2 G X 

• (x\ * x 2 ) * X3 = Xi * (X2 * x 3 ) 

• There exists an element I £ X such that I * x = x * I = x for all 
x £ X. 

• For every x £ X, there exists some element y £ X such that x *y = 
y * x = I. 


Show that SO(n) with the operation of matrix multiplication is a group. 


8. Derive Equations (2.6 1 and (2.7 1 . 

9. Suppose A is a 2 x 2 rotation matrix. In other words A T A = I and 
det A = 1. Show that there exists a unique 6 such that A is of the form 


A 


cos 9 — sin 8 

sin 6 cos 6 


10. Consider the following sequence of rotations: 

(a) Rotate by cf> about the world axaxis. 

(b) Rotate by 9 about the current z-axis. 

(c) Rotate by ijj about the world y-axis. 

Write the matrix product that will give the resulting rotation matrix (do 
not perform the matrix multiplication). 
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11. Consider the following sequence of rotations: 

(a) Rotate by <p about the world x-axis. 

(b) Rotate by 6 about the world 2 -axis. 

(c) Rotate by ip about the current x-axis. 

Write the matrix product that will give the resulting rotation matrix (do 
not perform the matrix multiplication). 

12. Consider the following sequence of rotations: 

(a) Rotate by <p about the world x-axis. 

(b) Rotate by 0 about the current 2 -axis. 

(c) Rotate by ip about the current x-axis. 

(d) Rotate by a about the world 2 -axis. 

Write the matrix product that will give the resulting rotation matrix (do 
not perform the matrix multiplication). 

13. Consider the following sequence of rotations: 

(a) Rotate by <p about the world x-axis. 

(b) Rotate by 0 about the world 2 -axis. 

(c) Rotate by ip about the current x-axis. 

(d) Rotate by a about the world 2 -axis. 

Write the matrix product that will give the resulting rotation matrix (do 
not perform the matrix multiplication). 

14. Find the rotation matrix representing a roll of J followed by a yaw of ^ 
followed by a pitch of f . 

15. If the coordinate frame 0 iX\y\Z\ is obtained from the coordinate frame 
oqXqVqZq by a rotation of about the a;-axis followed by a rotation of 
j about the fixed y-axis, find the rotation matrix R representing the 
composite transformation. Sketch the initial and final frames. 

16. Suppose that three coordinate frames o\Xiy\Z\, 02 X 2 IJ 2 Z 2 and 03 X 3 IJ 3 Z 3 
are given, and suppose 


' 1 

0 

1 

O 


' 0 

0 

-1 ' 

0 

1 

2 

VS 

2 

;*3 = 

0 

1 

0 

1 

0 

Vs 

2 

1 

2 


1 

0 

O 


Find the matrix i?|. 


17. Verify Equation (2.461. 
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18. 

19. 

20 . 

21 . 


22 . 

23. 

24. 

25. 


26. 


27. 


If R is a rotation matrix show that +1 is an eigenvalue of R. Let k be 
a unit eigenvector corresponding to the eigenvalue +1. Give a physical 
interpretation of k. 


Let k = Tf ^ 1 ’ 1 ’ 1 ) 5 


0 = 90°. Find R 


k,e- 


Show by direct calculation that R k g given by Equation (|2.46|) is equal to 


R given by Equation (2.511 if 9 and k are given by Equations (2.52) and 
(2.531, respectively. 


Compute the rotation matrix given by the product 


Rx ,9 Ry ,<f>Rz ,7r Ry (j>Rx , — 6 


Suppose R represents a rotation of 90° about y 0 followed by a rotation of 
45° about z\. Find the equivalent axis/angle to represent R. Sketch the 
initial and final frames and the equivalent axis vector k. 

Find the rotation matrix corresponding to the set of Euler angles { f , 0, \ }. 
What is the direction of the Xi axis relative to the base frame? 

Section [2 . 5 . 1 1 described only the Z-Y-Z Euler angles. List all possible sets 
of Euler angles. Is it possible to have Z-Z-Y Euler angles? Why or why 
not? 

Unit magnitude complex numbers (i.e., a + ib such that a 2 + b 2 = 1) 
can be used to represent orientation in the plane. In particular, for the 
complex number a + ib , we can define the angle 9 = atan2(a, b). Show 
that multiplication of two complex numbers corresponds to addition of 
the corresponding angles. 

Show that complex numbers together with the operation of complex mul- 
tiplication define a group. What is the identity for the group? What is 
the inverse for a + ibl 

Complex numbers can be generalized by defining three independent square 
roots for —1 that obey the multiplication rules 

-1 = * = J = k , 

i = jk = - kj , 
j = ki = — ik , 
k = ij = -ji 

Using these, we define a quaternion by Q = qo + iq\ + jq 2 + kq$, which 
is typically represented by the 4-tuple ( 90 , 9i, 92 , 93 )- A rotation by 9 
about the unit vector n = (n x ,n y ,n z ) T can be represented by the unit 
quaternion Q = (cos | , n x sin | , n y sin | , n z sin |) . Show that such a 
quaternion has unit norm, i.e., that 9o + 9i + 92 + Q 3 = 1- 
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28. Using Q = (cos | , n x sin | , n y sin | , n z sin |) , and the results from Section 
|2.5.3[ determine the rotation matrix R that corresponds to the rotation 
represented by the quaternion (go, <Zi, Q 2 , 93 )- 

29. Determine the quaternion Q that represents the same rotation as given 
by the rotation matrix R. 

30. The quaternion Q = (go, gi, < 72 , Q 3 ) can be thought of as having a scalar 
component go and a vector component = (gi, 92 , g 3 ) T - Show that the 
product of two quaternions, if = XY is given by 

To = x oUo - x T y 
z = x 0 y + y 0 x + xxy, 

Hint: perform the multiplication {xo+ix\+ jx 2 + kx^){yo+iyi+ jy 2 + kyo) 
and simplify the result. 

31. Show that Qi = (1,0, 0,0) is the identity element for unit quaternion 
multiplication, i.e., that QQj = QiQ = Q for any unit quaternion Q. 

32. The conjugate Q* of the quaternion Q is defined as 

Q * = (90,-91,-92,-93) 

Show that Q* is the inverse of Q , i.e., that Q*Q = QQ* = (1, 0, 0, 0). 

33. Let v be a vector whose coordinates are given by (v x , v y , v z ) T . If the 
quaternion Q represents a rotation, show that the new, rotated coor- 
dinates of v are given by Q(0, v x , v y , v z )Q* , in which (0,v x ,v y ,v z ) is a 
quaternion with zero as its real component. 

34. Let the point p be rigidly attached to the end effector coordinate frame 
with local coordinates ( x,y,z ). If Q specifies the orientation of the end 
effector frame with respect to the base frame, and T is the vector from 
the base frame to the origin of the end effector frame, show that the 
coordinates of p with respect to the base frame are given by 


Q(0,x,y,z)Q* +T 


(2.73) 


in which (0 ,x,y,z) is a quaternion with zero as its real component. 


35. Compute the homogeneous transformation representing a translation of 
3 units along the x-axis followed by a rotation of f about the current 
z-axis followed by a translation of 1 unit along the fixed y-axis. Sketch 
the frame. What are the coordinates of the origin 0\ with respect to the 
original frame in each case? 


36. Consider the diagram of Figure 2.15 


Find the homogeneous transfer- 
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Fig. 2.15 Diagram for Problem |2|36[ 




Fig. 2.16 Diagram for Problem |2|37| 


mations H®, H®, H), representing the transformations among the three 
frames shown. Show that H J? = H®, H\. 

37. Consider the diagram of Figure |2.16| A robot is set up 1 meter from a 
table. The table top is 1 meter high and 1 meter square. A frame 0 \X\yiZi 
is fixed to the edge of the table as shown. A cube measuring 20 cm on a 
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side is placed in the center of the table with frame 02X21/2X2 established at 
the center of the cube as shown. A camera is situated directly above the 
center of the block 2m above the table top with frame 03X32/3X3 attached 
as shown. Find the homogeneous transformations relating each of these 
frames to the base frame ooXo2/o~o- Find the homogeneous transformation 
relating the frame 02X22/2 22 to the camera frame 03X32/3^3. 

38. In Problem |37l suppose that, after the camera is calibrated, it is rotated 
90° about x 3. Recompute the above coordinate transformations. 

39. If the block on the table is rotated 90° about X2 and moved so that its 
center has coordinates (0, .8, .1) T relative to the frame 01X12/1X1, compute 
the homogeneous transformation relating the block frame to the camera 
frame; the block frame to the base frame. 

40. Consult an astronomy book to learn the basic details of the Earth’s rota- 
tion about the sun and about its own axis. Define for the Earth a local 
coordinate frame whose x-axis is the Earth’s axis of rotation. Define t = 0 
to be the exact moment of the summer solstice, and the global reference 
frame to be coincident with the Earth’s frame at time t = 0. Give an 
expression R(t) for the rotation matrix that represents the instantaneous 
orientation of the earth at time t. Determine as a function of time the 
homogeneous transformation that specifies the Earth’s frame with respect 
to the global reference frame. 

41. In general, multiplication of homogeneous transformation matrices is not 
commutative. Consider the matrix product 


H = R,ot x . Q Tr a,ns x . f,Tr ans z ; d . 0 

Determine which pairs of the four matrices on the right hand side com- 
mute. Explain why these pairs commute. Find all permutations of these 
four matrices that yield the same homogeneous transformation matrix, 
H. 



FORWARD AND 
INVERSE KINEMATICS 


In this chapter we consider the forward and inverse kinematics for serial link 
manipulators. The problem of kinematics is to describe the motion of the ma- 
nipulator without consideration of the forces and torques causing the motion. 
The kinematic description is therefore a geometric one. We first consider the 
problem of forward kinematics, which is to determine the position and orien- 
tation of the end-effector given the values for the joint variables of the robot. 
The inverse kinematics problem is to determine the values of the joint variables 
given the end-effector position and orientation. 


3.1 KINEMATIC CHAINS 

As described in Chapter [T] a robot manipulator is composed of a set of links 
connected together by joints. The joints can either be very simple, such as 
a revolute joint or a prismatic joint, or they can be more complex, such as a 
ball and socket joint. (Recall that a revolute joint is like a hinge and allows 
a relative rotation about a single axis, and a prismatic joint permits a linear 
motion along a single axis, namely an extension or retraction.) The difference 
between the two situations is that, in the first instance, the joint has only a 
single degree-of-freedom of motion: the angle of rotation in the case of a revolute 
joint, and the amount of linear displacement in the case of a prismatic joint. In 
contrast, a ball and socket joint has two degrees-of-freedom. In this book it is 
assumed throughout that all joints have only a single degree-of-freedom. This 
assumption does not involve any real loss of generality, since joints such as a ball 
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and socket joint (two degrees-of-freedom) or a spherical wrist (three degrees-of- 
freedom) can always be thought of as a succession of single degree-of-freedom 
joints with links of length zero in between. 

With the assumption that each joint has a single degree-of-freedom, the ac- 
tion of each joint can be described by a single real number; the angle of rotation 
in the case of a revolute joint or the displacement in the case of a prismatic 
joint. The objective of forward kinematic analysis is to determine the cumula- 
tive effect of the entire set of joint variables, that is, to determine the position 
and orientation of the end effector given the values of these joint variables. The 
objective of inverse kinematic analysis is, in contrast, to determine the values 
for these joint variables given the position and orientation of the end effector 
frame. 

A robot manipulator with n joints will have n + 1 links, since each joint 
connects two links. We number the joints from 1 to n, and we number the links 
from 0 to n , starting from the base. By this convention, joint i connects link 
i — 1 to link i. We will consider the location of joint i to be fixed with respect 
to link i — 1. When joint i is actuated, link i moves. Therefore, link 0 (the 
first link) is fixed, and does not move when the joints are actuated. Of course 
the robot manipulator could itself be mobile (e.g., it could be mounted on a 
mobile platform or on an autonomous vehicle), but we will not consider this 
case in the present chapter, since it can be handled easily by slightly extending 
the techniques presented here. 

With the i th joint, we associate a joint variable , denoted by g^. In the case 
of a revolute joint, qi is the angle of rotation, and in the case of a prismatic 
joint, q.i is the joint displacement: 

_ J 9i if joint i is revolute , , 

1 di if joint i is prismatic ' ’ 

To perform the kinematic analysis, we attach a coordinate frame rigidly 
to each link. In particular, we attach OiXi/yiZi to link i. This means that, 
whatever motion the robot executes, the coordinates of each point on link i 
are constant when expressed in the i th coordinate frame. Furthermore, when 
joint i is actuated, link i and its attached frame, OiXiyiZi , experience a resulting 
motion. The frame OqXqDqZq, which is attached to the robot base, is referred to 
as the inertial frame. Figure [3TT] illustrates the idea of attaching frames rigidly 
to links in the case of an elbow manipulator. 

Now suppose Ai is the homogeneous transformation matrix that expresses 
the position and orientation of OiXiyiZi with respect to Oi-iXi-iyt-iZi-i. The 
matrix A i is not constant, but varies as the configuration of the robot is changed. 
However, the assumption that all joints are either revolute or prismatic means 
that Ai is a function of only a single joint variable, namely qi. In other words, 

Ai = Ai(qi) (3.2) 

Now the homogeneous transformation matrix that expresses the position and 
orientation of OjXjyjZj with respect to OiXiyiZi is called, by convention, a trans- 
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Fig. 3.1 Coordinate frames attached to elbow manipulator 


formation matrix, and is denoted by T- . From Chapter pi we see that 



Ai + iA. i+2 ■ 

..Aj-iAj if * < j 

I 

if i = j 

c nr 1 

if 3 >i 


(3.3) 


By the manner in which we have rigidly attached the various frames to the 
corresponding links, it follows that the position of any point on the end-effector, 
when expressed in frame n , is a constant independent of the configuration of 
the robot. Denote the position and orientation of the end-effector with respect 
to the inertial or base frame by a three-vector o° (which gives the coordinates 
of the origin of the end-effector frame with respect to the base frame) and the 
3x3 rotation matrix and define the homogeneous transformation matrix 


H 


R° 

± L n 

o 


(3.4) 


Then the position and orientation of the end-effector in the inertial frame are 
given by 


H = T® = Ai(qi) ■ ■ ■ A n (q n ) 


Each homogeneous transformation Ai is of the form 


Ai 


0 1 


(3.5) 


(3.6) 


Hence 


68 FORWARD AND INVERSE KINEMATICS 


The matrix /?,* expresses the orientation of OjXjyjZj relative to OiXiijiZi and 
is given by the rotational parts of the ^4-matrices as 


R ) = 


R 


i + 1 


■R 


. 3 - 1 


(3.8) 


The coordinate vectors o* are given recursively by the formula 


1 


R 


'i-i 


of - 1 


(3.9) 


These expressions will be useful in Chapter [4] when we study Jacobian matrices. 

In principle, that is all there is to forward kinematics; determine the functions 
Ai(qi), and multiply them together as needed. However, it is possible to achieve 
a considerable amount of streamlining and simplification by introducing further 
conventions, such as the Denavit-Hartenberg representation of a joint, and this 
is the objective of the next section. 


3.2 FORWARD KINEMATICS: THE DENAVIT-HARTENBERG 
CONVENTION 

In this section we develop the forward or configuration kinematic equa- 
tions for rigid robots. The forward kinematics problem is concerned with the 
relationship between the individual joints of the robot manipulator and the po- 
sition and orientation of the tool or end-effector. The joint variables are the 
angles between the links in the case of revolute or rotational joints, and the link 
extension in the case of prismatic or sliding joints. 

We will develop a set of conventions that provide a systematic procedure 
for performing this analysis. It is, of course, possible to carry out forward 
kinematics analysis even without respecting these conventions, as we did for 
the two-link planar manipulator example in Chapter [T] However, the kinematic 
analysis of an n-link manipulator can be extremely complex and the conventions 
introduced below simplify the analysis considerably. Moreover, they give rise 
to a universal language with which robot engineers can communicate. 

A commonly used convention for selecting frames of reference in robotic 
applications is the Denavit-Hartenberg, or DH convention. In this convention, 
each homogeneous transformation Ai is represented as a product of four basic 
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transformations 

Ai = Rot z a i Tra,ns Z 'd i Tra,ias x „ i Rot x 


(3.10) 
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where the four quantities 0i , ai, di, ai are parameters associated with link i 


and joint i. The four parameters ai,ai,di, and di in (3.101 are generally given 
the names link length, link twist, link offset, and joint angle, respectively. 
These names derive from specific aspects of the geometric relationship between 
two coordinate frames, as will become apparent below. Since the matrix Ai is a 
function of a single variable, it turns out that three of the above four quantities 
are constant for a given link, while the fourth parameter, di for a revolute joint 
and di for a prismatic joint, is the joint variable. 

From Chapter [2] one can see that an arbitrary homogeneous transformation 
matrix can be characterized by six numbers, such as, for example, three numbers 
to specify the fourth column of the matrix and three Euler angles to specify the 
upper left 3x3 rotation matrix. In the DH representation, in contrast, there 
are only four parameters. How is this possible? The answer is that, while frame 
i is required to be rigidly attached to link i, we have considerable freedom in 
choosing the origin and the coordinate axes of the frame. For example, it is not 
necessary that the origin, o i? of frame i be placed at the physical end of link i. 
In fact, it is not even necessary that frame i be placed within the physical link; 
frame i could lie in free space — so long as frame i is rigidly attached to link 
i. By a clever choice of the origin and the coordinate axes, it is possible to cut 
down the number of parameters needed from six to four (or even fewer in some 


cases). In Section 3.2.1 we will show why, and under what conditions, this can 


be done, and in Section 3.2.2 we will show exactly how to make the coordinate 
frame assignments. 


3.2.1 Existence and uniqueness issues 

Clearly it is not possible to represent any arbitrary homogeneous transformation 
using only four parameters. Therefore, we begin by determining just which 


homogeneous transformations can be expressed in the form (3.10l. Suppose we 
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Fig. 3.2 Coordinate frames satisfying assumptions DH1 and DH2 


are given two frames, denoted by frames 0 and 1, respectively. Then there exists 
a unique homogeneous transformation matrix A that takes the coordinates from 
frame 1 into those of frame 0. Now suppose the two frames have the following 
two additional features. 

DH Coordinate Frame Assumptions 
(DH1) The axis X\ is perpendicular to the axis Zo- 
(DH2) The axis x± intersects the axis zq. 

These two properties are illustrated in Figure [372) Under these conditions, we 
claim that there exist unique numbers a, d, 9, a such that 


A = Rot z gTrans Z i]T^SLTxs xa Rot Xta (3-11) 


Of course, since 6 and a are angles, we really mean that they are unique to 
within a multiple of 27 t. To show that the matrix A can be written in this form, 
write A as 


A 


0 1 


(3.12) 


If (DH1) is satisfied, then x\ is perpendicular to zq and we have x\ ■ zq = 0. 
Expressing this constraint with respect to oox^yoZo, using the fact that the first 
column of is the representation of the unit vector x\ with respect to frame 
0, we obtain 

0 = x? • zH 

0 
0 
1 


[Oi,»"2i,r3i] 


= r 3 1 
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Since r 3 i = 0, we now need only show that there exist unique angles 9 and a 
such that 


R°i = 


JD JD 

- n TE,0- r *':r, a. 


ce 

So 

0 


SQCot 

CO C a 
S/y 


sos& 

CQ S c 
Cry 


(3.13) 


The only information we have is that ?~ 3 i =0, but this is enough. First, since 
each row and column of R® must have unit length, r 3 i = 0 implies that 


2 , 2 
r ii + tv 


' n 
2 


= 1, 

' 32 T ' 33 = 1 

Hence there exist unique 6 and a such that 

(bb^i) = (ce,s g ), (r 33 ,r 32 ) = ( c a ,s a ) 

Once 6 and a are found, it is routine to show that the remaining elements of 
Ri must have the form shown in (3.13), using the fact that R® is a rotation 
matrix. 

Next, assumption (DH2) means that the displacement between o 0 and o 1 
can be expressed as a linear combination of the vectors z o and x±. This can be 
written as o 1 = o 0 + dz 3 + ax\. Again, we can express this relationship in the 
coordinates of ooiro 2 /o~ 0 ) and we obtain 

°i = °o + d-o + ax i 

0 " 

0 
0 

QjCq 

as e 
d 



' 0 ' 


ce 

+ d 

0 

+ a 

se 


1 


0 


Combining the above results, we obtain (3.101 as claimed. Thus, we see that 
four parameters are sufficient to specify any homogeneous transformation that 
satisfies the constraints (DH1) and (DH2). 

Now that we have established that each homogeneous transformation matrix 
satisfying conditions (DH1) and (DH2) above can be represented in the form 
(3.101, we can in fact give a physical interpretation to each of the four quantities 


in (3.10l. The parameter a is the distance between the axes zq and z i, and is 
measured along the axis X\ . The angle a is the angle between the axes zq and 
z\, measured in a plane normal to x±. The positive sense for a is determined 
from zq to z\ by the right-handed rule as shown in Figure |3.3| The parameter 
d is the perpendicular distance from the origin o 0 to the intersection of the X\ 
axis with Zq measured along the zq axis. Finally, 9 is the angle between xq and 
X\ measured in a plane normal to zq- These physical interpretations will prove 
useful in developing a procedure for assigning coordinate frames that satisfy 
the constraints (DH1) and (DH2), and we now turn our attention to developing 
such a procedure. 
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Fig. 3.3 Positive sense for on and 9i 


3.2.2 Assigning the coordinate frames 


For a given robot manipulator, one can always choose the frames 0, . . . , n in 
such a way that the above two conditions are satisfied. In certain circumstances, 
this will require placing the origin o i of frame i in a location that may not be 
intuitively satisfying, but typically this will not be the case. In reading the 
material below, it is important to keep in mind that the choices of the various 
coordinate frames are not unique, even when constrained by the requirements 
above. Thus, it is possible that different engineers will derive differing, but 
equally correct, coordinate frame assignments for the links of the robot. It is 
very important to note, however, that the end result (i.e., the matrix T°) will 
be the same, regardless of the assignment of intermediate link frames (assuming 
that the coordinate frames for link n coincide). We will begin by deriving the 
general procedure. We will then discuss various common special cases where it 
is possible to further simplify the homogeneous transformation matrix. 


To start, note that the choice of z,; is arbitrary. In particular, from (3.131, 


we see that by choosing and appropriately, we can obtain any arbitrary 
direction for Zj. Thus, for our first step, we assign the axes Zq, , z n -\ in an 
intuitively pleasing fashion. Specifically, we assign z-i to be the axis of actuation 
for joint i + 1. Thus, Zq is the axis of actuation for joint 1, Z\ is the axis of 
actuation for joint 2, etc. There are two cases to consider: (i) if joint i + 1 is 
revolute, z t is the axis of revolution of joint i + 1; (ii) if joint i + 1 is prismatic, 
Zi is the axis of translation of joint i + 1. At first it may seem a bit confusing 
to associate z, with joint i + 1 , but recall that this satisfies the convention that 
we established above, namely that joint i is fixed with respect to frame i, and 
that when joint i is actuated, link i and its attached frame, OiXii/iZi, experience 
a resulting motion. 

Once we have established the z-axes for the links, we establish the base frame. 
The choice of a base frame is nearly arbitrary. We may choose the origin o 0 of 
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the base frame to be any point on zq. We then choose Xq, yo in any convenient 
manner so long as the resulting frame is right-handed. This sets up frame 0. 

Once frame 0 has been established, we begin an iterative process in which 
we define frame i using frame i — 1, beginning with frame 1. Figure 3.4 will be 
useful for understanding the process that we now describe. 


I 



Fig. 3.4 Denavit-Hartenberg frame assignment 

In order to set up frame i it is necessary to consider three cases: (i) the axes 
Zi- 1 , Zi are not coplanar, (ii) the axes i, z % intersect (iii) the axes Zi-i, Zi are 
parallel. Note that in both cases (ii) and (iii) the axes Zj_i and are coplanar. 
This situation is in fact quite common, as we will see in Section [3. 2. 3 1 We now 
consider each of these three cases. 

(i) Zi - 1 and Zi are not coplanar: If Zi-i and Zi are not coplanar, then there exists 
a unique line segment perpendicular to both z ^- 1 and z., such that it connects 
both lines and it has minimum length. The line containing this common normal 
to Zi - 1 and Zi defines Xi, and the point where this line intersects z t is the origin 
o i . By construction, both conditions (DH1) and (DH2) are satisfied and the 
vector from o i _ 1 to o i is a linear combination of zi-\ and a The specification of 
frame i is completed by choosing the axis yi to form a right-handed frame. Since 
assumptions (DH1) and (DH2) are satisfied the homogeneous transformation 
matrix A; is of the form ( |3A0| ). 

(ii) Zi - 1 is parallel to zc. If the axes Zj_i and Zi are parallel, then there are 
infinitely many common normals between them and condition (DH1) does not 
specify Xi completely. In this case we are free to choose the origin o t anywhere 
along Zi . One often chooses o i to simplify the resulting equations. The axis 
Xi is then chosen either to be directed from o i toward Zj_i, along the common 
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normal, or as the opposite of this vector. A common method for choosing o i 
is to choose the normal that passes through o i _ 1 as the a axis; o i is then the 
point at which this normal intersects z\. In this case, di would be equal to zero. 
Once Xi is fixed, yt is determined, as usual by the right hand rule. Since the 
axes Zi - 1 and z % are parallel, cq will be zero in this case. 


(Hi) Zi-i intersects z^: In this case is chosen normal to the plane formed 
by Zi and Z{-\. The positive direction of a is arbitrary. The most natural 
choice for the origin o i in this case is at the point of intersection of z t and Zi-\. 
However, any convenient point along the axis z t suffices. Note that in this case 
the parameter cq equals 0. 

This constructive procedure works for frames 0, . . . , n — 1 in an n-link robot. 
To complete the construction, it is necessary to specify frame n. The final 
coordinate system o n x n y n z n is commonly referred to as the end-effector or 
tool frame (see Figure 3.5 1 . The origin o n is most often placed symmetrically 
between the fingers of the gripper. The unit vectors along the x n , y n , and z n 
axes are labeled as n, s, and a, respectively. The terminology arises from fact 
that the direction a is the approach direction, in the sense that the gripper 
typically approaches an object along the a direction. Similarly the s direction is 
the sliding direction, the direction along which the fingers of the gripper slide 
to open and close, and n is the direction normal to the plane formed by a and 



In most contemporary robots the final joint motion is a rotation of the end- 
effector by 8 n and the final two joint axes, z n - 1 and z n , coincide. In this case, 
the transformation between the final two coordinate frames is a translation 
along z n - 1 by a distance d n followed (or preceded) by a rotation of 9 n about 
z n -\. This is an important observation that will simplify the computation of 
the inverse kinematics in the next section. 

Finally, note the following important fact. In all cases, whether the joint in 
question is revolute or prismatic, the quantities ai and on are always constant 
for all i and are characteristic of the manipulator. If joint i is prismatic, then 
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9 i is also a constant, while di is the i th joint variable. Similarly, if joint i is 
revolute, then di is constant and Oi is the i th joint variable. 

3.2.3 Examples 

In the DH convention the only variable angle is 6 , so we simplify notation by 
writing a for cos Oi, etc. We also denote Q\ + O2 by O12, and cos($i + 0 2 ) by C12, 
and so on. In the following examples it is important to remember that the DH 
convention, while systematic, still allows considerable freedom in the choice of 
some of the manipulator parameters. This is particularly true in the case of 
parallel joint axes or when prismatic joints are involved. 

Example 3.1 Planar Elbow Manipulator 



Fig. 3.6 Two-link planar manipulator. The 2 -axes all point out of the page, and are 
not shown in the figure 


Consider the two-link planar arm of Figure \ 3 . 6 \ The joint axes Zq and Z\ 
are normal to the page. We establish the base frame ooXoyoZo as shown. The 
origin is chosen at the point of intersection of the zq axis with the page and 
the direction of the Xq axis is completely arbitrary. Once the base frame is 
established, the 0iX\y\Z\ frame is fixed as shown by the DH convention, where 
the origin o ± has been located at the intersection of Zi and the page. The final 
frame 02222/2-2 fixed by choosing the origin o 2 at the end of link 2 as shown. 
The DH parameters are shown in Table 3.1 The A-matrices are determined 
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Table 3.1 Link parameters for 2-link planar manipulator 


Link 

cii 

Oil 

di 

Bi 

1 

ai 

0 

0 

9\ 

2 

a 2 

0 

0 

9*2 


* variable 


from (3.10) as 


M = 


Ao — 


Cl 

-Sl 

0 

aic-i 

Si 

Cl 

0 

aisi 

0 

0 

1 

0 

0 

0 

0 

1 

c 2 

s 2 

0 

a 2 c 2 

s 2 

C 2 

0 

a 2 s 2 

0 

0 

1 

0 

0 

0 

0 

1 


The T -matrices are thus given by 

T? = A! 


T 2 ° = A!A 2 = 


c 12 
s 12 

0 

0 


-Sl2 

Cl2 

0 

0 


aiCi + a 2 Ci2 
aiSi + a 2 Si2 
0 
1 


Notice that the first two entries of the last column of T 2 are the x and y 
components of the origin o 2 in the base frame; that is, 

x = aiCi + a 2 Ci 2 
y = aiSi + a 2 S!2 

are the coordinates of the end-effector in the base frame. The rotational part of 
T 2 ° gives the orientation of the frame o 2 x 2 y 2 Z 2 relative to the base frame, 
o 


Example 3.2 Three-Link Cylindrical Robot 


Consider now the three-link 
We establish o n as 


cylindrical robot represented symbolically by Figure 3.7 
shown at joint 1. Note that the placement of the origin o 0 along Zq as well as 
the direction of the Xo axis are arbitrary. Our choice of o 0 is the most natural , 
but o 0 could just as well be placed at joint 2. The axis Xq is chosen normal 
to the page. Next, since Zq and Z\ coincide, the origin o x is chosen at joint 1 
as shown. The X\ axis is normal to the page when 9\ = 0 but, of course its 
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Table 3.2 Link parameters for 3-link cylindrical manipulator 


Link 

CLi 

OLi 

di 

0t 

1 

0 

0 

di 

6 * 

2 

0 

-90 

d* 2 

0 

3 

0 

0 

d* 3 

0 


* variable 


direction will change since 9\ is variable. Since z 2 and zi intersect, the origin 
o 2 is placed at this intersection. The direction of X 2 is chosen parallel to x\ 
so that 9 2 is zero. Finally, the third frame is chosen at the end of link 3 as 

The corresponding A and 


shown. The DH parameters are shown in Table 3.2 



X 2 


C?2 


Xi 


X 0 




e 1 



y 0 


£3 


Fig. 3. 7 Three-link cylindrical manipulator 
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T matrices are 


A i 


A 2 


A3 


ci —si 0 0 

si Ci 0 0 

0 0 1 ch 

0 0 0 1 

1 0 0 0 ' 

0 0 10 

0 -1 0 d 2 

0 0 0 1 

1 0 0 0 ' 

0 10 0 

0 0 1 d 3 

0 0 0 1 


T 3 0 = A3A 2 A 3 


ci 0 —Si — sid 3 

Si 0 ci cid3 

0—1 0 di + d 2 

0 0 0 1 


o 

Example 3.3 Spherical Wrist 


£3, * 

X H *7\6* 




~r, 


z 4 


To gripper 


( 3 . 14 ) 


Fig. 3.8 The spherical wrist frame assignment 


The spherical wrist configuration is shown in Figure | ■?.<$[ in which the joint 

The 


axes z 3, Z4, Z5 intersect at o. The DH parameters are shown in Table T3 


Stanford manipulator is an example of a manipulator that possesses a wrist of 
this type. 

We show now that the final three joint variables, 64, 9 5 , 9 e are the Euler 
angles <f>, 9, if, respectively, with respect to the coordinate frame 03X32/32:3. To 
see this we need only compute the matrices A4, A$, and Aq using Table 3.3 and 
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Table 3.3 DH parameters for spherical wrist 


Link 

Oi 

Cti 

di 

e t 

4 

0 

-90 

0 

o % 

5 

0 

90 

0 

% 

6 

0 

0 

de 

01 


* variable 


the expression (3.10). 


This gives 


A 4 


A5 


Aq 


C 4 

0 

-Si 

0 

S 4 

0 

c 4 

0 

0 

-1 

0 

0 

0 

0 

0 

1 

C5 

0 

^5 

0 

S5 

0 

-C 5 

0 

0 

-1 

0 

0 

0 

0 

0 

1 

C6 

-S6 

0 

0 

S6 

C6 

0 

0 

0 

0 

1 

de 

0 

0 

0 

1 


Multiplying these together yields 
Tq = A 4 A$Aq 

' Rl 4 ' 

0 1 


C4C5C6 S4S6 

C4C5S6 S4 Cq 

C 4 S5 

C4 556^6 

S4C5C6 + C4S6 

S4C556 + C4CQ 

S 4 S 5 

5455(^6 

55C6 

£5^6 

C5 

C$dQ 

0 

0 

0 

1 


Comparing the rotational part ofT, f with the Euler angle transformation 


(2.21) shows that 0 4 ,0c,,0e can indeed be identified as the Euler angles <f>, 0 and 


if with respect to the coordinate frame o^x^y^z^. 


Example 3.4 Cylindrical Manipulator with Spherical Wrist 

Suppose that we now attach a spherical wrist to the cylindrical manipulator 
of Example 3.2 as shown in Figure |iO[ Note that the axis of rotation of joint f 
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A 


G?3 ^ #5 



is parallel to z^ and thus coincides with the axis z$ of Example \3.2\ 
cation of this is that we can immediately combine the two previous 
(3. If) and (3.15) to derive the forward kinematics as 


The impli- 
expression 


t 6 ° = t 3 °t 6 3 


(3.16) 


with Tg given by ( 3. If ) and Xg given by (3.15). 
matics of this manipulator is described by 


Therefore the forward kine- 


r 11 

I"12 

r 13 

dx 

T21 

T22 

T23 

dy 

T31 

T32 

T 33 

d z 

0 

0 

0 

1 


rr iO 
1 6 


(3.17) 
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k 




Note: the shoulder (prismatic joint) is mounted wrong. 


Fig. 3.10 DH coordinate frame assignment for the Stanford manipulator 


in which 


rn = C1C4C5C6 — C1S4S6 + S1S5C6 

T21 = S1C4C5C6 — S1S4S6 — C1S5C6 

T31 = — S 4 C 5 C 6 — C 4 S 6 

r 12 = — C1C4C5S6 — C1S4C6 — S1S5C6 

T 22 = SiC&C^Sq — S1S4SQ + C1S5C6 

f32 = S4C5C6 — C4C6 

T 13 = C1C4S5 — S1C5 

^23 = S1C4S5 + C1C5 

T 33 = S 4 S 5 

d x cic^s^djQ sic^cIq Si^3 

dy — siCiScydfi -\- Cic^dfj -\- Cid$ 

d z = - S4.s 5 d e + di + d 2 


Notice how most of the complexity of the forward kinematics for this manip- 
ulator results from the orientation of the end- effector while the expression for 
the arm position from (3.14) is fairly simple. The spherical wrist assumption 
not only simplifies the derivation of the forward kinematics here, but will also 
greatly simplify the inverse kinematics problem in the next chapter. 


Example 3.5 Stanford Manipulator 

Consider now the Stanford Manipulator shown in Figure \3.10\ This manip- 
ulator is an example of a spherical (RRP) manipulator with a spherical wrist. 
This manipulator has an offset in the shoulder joint that slightly complicates 
both the forward and inverse kinematics problems. 
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Table 3.4 DH parameters for Stanford Manipulator 


Link 

di 

ai 

OLi 

0i 

1 

0 

0 

-90 

9* 

2 

d 2 

0 

+90 

9* 

3 

d * 

0 

0 

0 

4 

0 

0 

-90 

9* 

5 

0 

0 

+90 

9* 

6 

d& 

0 

0 

9* 


* joint variable 


We first establish the joint coordinate frames using the DH convention as 
shown. The DH parameters are shown in the Table \37j\ 

It is straightforward to compute the matrices Ai as 


Ai 


A2 


A3 


A4 


A5 


^6 


ci 0 —si 0 

si 0 ci 0 

0-1 0 0 

0 0 0 1 

c 2 0 s 2 0 

s 2 0 — c 2 0 

0 1 0 d 2 

0 0 0 1 

10 0 O' 

0 10 0 

0 0 1 d 3 

0 0 0 1 

C 4 0 — S 4 0 

S 4 0 C 4 0 

0-1 0 0 

0 0 0 1 

c 5 0 s 5 0 

s 5 0 — c 5 0 

0-1 0 0 

0 0 0 1 

Cq Sq 0 0 

Sg Cg 0 0 

0 0 1 d 6 

0 0 0 1 


(3.18) 

(3.19) 

(3.20) 

(3.21) 

(3.22) 


(3.23) 
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Tg is then given as 


A!--- 

Aq 



(3.24) 

rn 

ri2 

T\3 

dx 


^21 

T22 

T23 

dy 

7 

(3.25) 

r 3 i 

T32 

T33 

d z 

0 

0 

0 

1 



where 

T\\ = C\ [c 2 (C4C5C6 — S4S6) — S2S5C6] — d2(S4C5C6 + C4S6) 

f21 = Si[c2(C4C5C6 — S4Sq) — S2S5C6] + Ci(S4CsC6 + C4Sq) 

T31 = — S2(C4C5C6 — S4S6) — C2S5C6 

ri2 = Ci[—C2{C4C5Sq + S4Cq) + S2S5Sq} — Si(—S4C5Sq + C4Cq) 
r 2 2 = ^Sl C 2 (C4C5Sg + S4Cg) + S2S5S6] + Ci( — S4C5S6 + C4C6) 

T32 = S 2 (C4C5S6 + S 4 C 6 ) + C 2 S 5 S 6 

T 13 = Ci(c 2 C4S5 + S2C5) — S1S4S5 

T23 = Si(c2C4S5 + S2C5) + C1S4S5 

T33 = ^2^4^5 + C 2C5 

da; = CiS 2 d3 — SlC? 2 + +dg(CiC2C4S5 + C1C5S2 ~ S1S4S5) 
dy = Sis 2 d 3 + Cid 2 + de(ciS4S3 + C2C4S1S5 + C5S1S2) 
dz = C 2 d 3 + d 6 (c 2 c 5 - C4S2S5) 


Example 3.6 SCARA Manipulator 

As another example of the general procedure, consider the SCARA manipula- 
tor of Figure \3. 11 This manipulator, which is an abstraction of the AdeptOne 
robot of Figure l.lf | consists of an RRP arm and a one degree- of- freedom wrist, 
whose motion is a roll about the vertical axis. The first step is to locate and 
label the joint axes as shown. Since all joint axes are parallel we have some 
freedom in the placement of the origins. The origins are placed as shown for 
convenience. We establish the xq axis in the plane of the page as shown. This is 
completely arbitrary and only affects the zero configuration of the manipulator, 
that is, the position of the manipulator when 6\ = 0. 

and the A-matrices are as fol- 


The joint parameters are given in Table 3.5 
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Zl 



Z3, Z4 


Fig. 3.11 DH coordinate frame assignment for the SCARA manipulator 
Table 3.5 Joint parameters for SCARA 


Link 

di 

Oil 

di 

e t 

1 

di 

0 

0 

6 * 

2 

a 2 

180 

0 

e * 

3 

0 

0 

d * 

0 

4 

0 

0 

C?4 

9 * 


* joint variable 


lows. 


Cl 

-Si 

0 

aici 

Si 

Cl 

0 

aiSi 

0 

0 

1 

0 

0 

0 

0 

1 

C2 

S2 

0 

a 2 c 2 

S2 

— c 2 

0 

a 2 s 2 

0 

0 

-1 

0 

0 

0 

0 

1 


10 0 0 

0 10 0 

0 0 1 d 3 

0 0 0 1 

C 4 — S 4 0 0 

S 4 C 4 0 0 

0 0 1 d 4 

0 0 0 1 



(3.26) 

(3.27) 

(3.28) 

(3.29) 
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rpO 

1 4 


The forward kinematic equations are therefore given by 
= A\ ■ ■ ■ A 4 

C12C4 + S12S4 — C12S4 + S12C4 0 

s 12 c 4 — c 12 s 4 ~ s 12 s 4 ~ c 12 c 4 0 

0 0-1 

0 0 0 


CliCi + (I2C12 
CllSl + d2 s 12 

-d 3 - d 4 

1 


(3.30) 


3.3 INVERSE KINEMATICS 

In the previous section we showed how to determine the end-effector position 
and orientation in terms of the joint variables. This section is concerned with 
the inverse problem of finding the joint variables in terms of the end-effector 
position and orientation. This is the problem of inverse kinematics, and it 
is, in general, more difficult than the forward kinematics problem. 

In this chapter, we begin by formulating the general inverse kinematics prob- 
lem. Following this, we describe the principle of kinematic decoupling and how 
it can be used to simplify the inverse kinematics of most modern manipula- 
tors. Using kinematic decoupling, we can consider the position and orientation 
problems independently. We describe a geometric approach for solving the po- 
sitioning problem, while we exploit the Euler angle parameterization to solve 
the orientation problem. 


3.3.1 The General Inverse Kinematics Problem 


The general problem of inverse kinematics can be stated as follows. Given a 
4x4 homogeneous transformation 


H = 


R o 
0 1 


G SE( 3) 


with R € 50(3), find (one or all) solutions of the equation 

T^ qi ,...,q n ) = H 

where 

t °(9i,---,<2Vi) = M{q\) ■ ■ ■ A n {q n ) 


(3.31) 


(3.32) 

(3.33) 


Here, H represents the desired position and orientation of the end-effector, 
and our task is to find the values for the joint variables qi,...,q n so that 

n'o/i //• 

Equation (3.32 1 results in twelve nonlinear equations in n unknown variables, 
which can be written as 


Tij (Ql , ■ - * ; qn) — dj j , 


* = 1,2,3, .7 = 1,..., 4 


(3.34) 
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where T,j , hij refer to the twelve nontrivial entries of and H, respectively. 
(Since the bottom row of both and H are (0,0, 0,1), four of the sixteen 
equations represented by (3.32) are trivial.) 


Example 3.7 

Recall the Stanford manipulator of Example 3.3.5. Suppose that the desired 
position and orientation of the final frame are given by 


H = 


0 10 
0 0 1 
10 0 
0 0 0 


-0.154 

0.763 

0 

1 


(3.35) 


To find the corresponding joint variables 9\, 9 2 , d 3 , 64 , 9§, and 9 q we must solve 
the following simultaneous set of nonlinear trigonometric equations: 


Ci[C2(C4C5C6 — S 4 S 6 ) — S 2 S 5 C 6 ] — Si(s4C5C6 + C 4 S 6 ) 
Si[c2(C4C5C6 — S4S6) ~ S2S5C6] + Ci (S4C5C6 + C4S6) 
— S2(C4C5C6 — S4S6) ~ C2S5C6 
Cl[— C2(C4C5S6 + S4C6) + S2S5S6] _ Sl( — S4C5S6 + C4C6) 
Sl[— C2(C4C5S6 + S4C6) + S2S5S6] + Ci( — S4C5S6 + C4C6) 

^2 (C 4 C 5 S 6 + S 4 C 6 ) + C 2 S 5 S 6 
Cl (C2C4S5 + S2C5) — S1S4S5 
^1 (C2C4S5 + S2C5) + C1S4S5 
— S2C4S5 + C2C5 

ClS2^3 — 51^2 + 4(C1C 2 C 4 S 5 + C1C5S2 — S1S4S5) 
s l s 2^3 + c ld 2 + <^6( C 1 S 4 S 5 + C 2 C 4 S 1 S 5 + C 5 S 1 S 2 ) 
C2G?3 + de(c 2 C5 — C4S2S5) 


0 

0 

1 

1 

0 

0 

0 

1 

0 

-0.154 

0.763 

0 


If the values of the nonzero DH parameters are d 2 = 0.154 and d$ = 0.263, 
one solution to this set of equations is given by: 

0i = tt/2, 9 2 = tt/2, d 3 = 0.5, 9 A = tt/2, 4 = 0, 9 6 = tt/2. 

Even though we have not yet seen how one might derive this solution, it is 
not difficult to verify that it satisfies the forward kinematics equations for the 
Stanford arm. 
o 

The equations in the preceding example are, of course, much too difficult to 
solve directly in closed form. This is the case for most robot arms. Therefore, 
we need to develop efficient and systematic techniques that exploit the partic- 
ular kinematic structure of the manipulator. Whereas the forward kinematics 
problem always has a unique solution that can be obtained simply by evaluating 
the forward equations, the inverse kinematics problem may or may not have a 
solution. Even if a solution exists, it may or may not be unique. Furthermore, 
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because these forward kinematic equations are in general complicated nonlin- 
ear functions of the joint variables, the solutions may be difficult to obtain even 
when they exist. 

In solving the inverse kinematics problem we are most interested in finding a 
closed form solution of the equations rather than a numerical solution. Finding 
a closed form solution means finding an explicit relationship: 


qk = fk(hn, ■ . . ,h 3i ), k = l,...,n (3.36) 


Closed form solutions are preferable for two reasons. First, in certain applica- 
tions, such as tracking a welding seam whose location is provided by a vision 
system, the inverse kinematic equations must be solved at a rapid rate, say ev- 
ery 20 milliseconds, and having closed form expressions rather than an iterative 
search is a practical necessity. Second, the kinematic equations in general have 
multiple solutions. Having closed form solutions allows one to develop rules for 
choosing a particular solution among several. 

The practical question of the existence of solutions to the inverse kinematics 
problem depends on engineering as well as mathematical considerations. For 
example, the motion of the revolute joints may be restricted to less than a full 
360 degrees of rotation so that not all mathematical solutions of the kinematic 
equations will correspond to physically realizable configurations of the manip- 
ulator. We will assume that the given position and orientation is such that at 


least one solution of (3.321 exists. Once a solution to the mathematical equa- 


tions is identified, it must be further checked to see whether or not it satisfies 
all constraints on the ranges of possible joint motions. For our purposes, we 


henceforth assume that the given homogeneous matrix H in (3.321 corresponds 


to a configuration within the manipulator’s workspace with an attainable ori- 
entation. This guarantees that the mathematical solutions obtained correspond 
to achievable configurations. 


3.3.2 Kinematic Decoupling 

Although the general problem of inverse kinematics is quite difficult, it turns out 
that for manipulators having six joints, with the last three joints intersecting at 
a point (such as the Stanford Manipulator above), it is possible to decouple the 
inverse kinematics problem into two simpler problems, known respectively, as 
inverse position kinematics, and inverse orientation kinematics. To put 
it another way, for a six-DOF manipulator with a spherical wrist, the inverse 
kinematics problem may be separated into two simpler problems, namely first 
finding the position of the intersection of the wrist axes, hereafter called the 
wrist center, and then finding the orientation of the wrist. 

For concreteness let us suppose that there are exactly six degrees-of-freedom 


and that the last three joint axes intersect at a point o c . We express (3.321 as 
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two sets of equations representing the rotational and positional equations 

R 


> Ota) — 
o%{qii ,q&) = 


(3.37) 

(3.38) 


where o and R are the desired position and orientation of the tool frame, ex- 
pressed with respect to the world coordinate system. Thus, we are given o and 
R , and the inverse kinematics problem is to solve for qi , . . . , q§. 

The assumption of a spherical wrist means that the axes £ 3 , z 4 , and z 3 
intersect at o c and hence the origins o 4 and o 5 assigned by the DH-convention 
will always be at the wrist center o c . Often o 3 will also be at o c , but this is 
not necessary for our subsequent development. The important point of this 
assumption for the inverse kinematics is that motion of the final three links 
about these axes will not change the position of o c , and thus, the position of 
the wrist center is thus a function of only the first three joint variables. 

The origin of the tool frame (whose desired coordinates are given by o) is 
simply obtained by a translation of distance de along z 3 from o c (see Table 3.3 1. 
In our case, z 3 and zq are the same axis, and the third column of R expresses 
the direction of zq with respect to the base frame. Therefore, we have 


o = o„ 


■ df:R 


(3.39) 


Thus in order to have the end-effector of the robot at the point with coordinates 
given by o and with the orientation of the end-effector given by R = (r,j ) , it is 
necessary and sufficient that the wrist center o c have coordinates given by 


= o — de.R 


(3.40) 


and that the orientation of the frame oqXqijqZq with respect to the base be given 
by R. If the components of the end-effector position o are denoted o x , o vl o z 
and the components of the wrist center o° c are denoted x c , y c ,z c then (3.401 
gives the relationship 


(3.41) 


x c 


o x ~ dgr 13 

Vc 

= 

O y — d§T23 

z c 


o z - d 6 r 33 


Using Equation ( 3.41 1 we may find the values of the first three joint variables. 
This determines the orientation transformation R° which depends only on these 
first three joint variables. We can now determine the orientation of the end- 
effector relative to the frame o 3 x 3 y 3 z 3 from the expression 


R 


r> 0 d 3 

— 


(3.42) 


INVERSE KINEMATICS 89 


as 

Rl = (R° 3 )- 1 R = (R° 3 ) T R (3.43) 

As we shall see in Section [3.3.4| the final three joint angles can then be found 
as a set of Euler angles corresponding to Rq. Note that the right hand side of 
) is completely known since R is given and can be calculated once 
the first three joint variables are known. The idea of kinematic decoupling is 
illustrated in Figure |3.12| 




3.3.3 Inverse Position: A Geometric Approach 


For the common kinematic arrangements that we consider, we can use a ge- 
ometric approach to find the variables, q \ , < 72 , 93 corresponding to o° c given by 
(3.401. We restrict our treatment to the geometric approach for two reasons. 
First, as we have said, most present manipulator designs are kinematically 
simple, usually consisting of one of the five basic configurations of Chapter [T] 
with a spherical wrist. Indeed, it is partly due to the difficulty of the gen- 
eral inverse kinematics problem that manipulator designs have evolved to their 
present state. Second, there are few techniques that can handle the general in- 
verse kinematics problem for arbitrary configurations. Since the reader is most 
likely to encounter robot configurations of the type considered here, the added 
difficulty involved in treating the general case seems unjustified. The interested 
reader can find more detailed treatment of the general case in [32 [34] [61] [72j . 

In general the complexity of the inverse kinematics problem increases with 
the number of nonzero link parameters. For most manipulators, many of the 
di , di are zero, the cti are 0 or ±7r/2, etc. In these cases especially, a geometric 
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approach is the simplest and most natural. The general idea of the geometric 
approach is to solve for joint variable q t by projecting the manipulator onto the 
Xi - 1 — yi - 1 plane and solving a simple trigonometry problem. For example, to 
solve for 8 1 , we project the arm onto the Xq — yo plane and use trigonometry 
to find 8\. We will illustrate this method with two important examples: the 
articulated and spherical arms. 


3.3.3. 1 
Figure 


Articulated Configuration Consider the elbow manipulator shown in 
with the components of o° c denoted by x c ,y c ,z c . We project o c 


3.13 


onto the xq — y o plane as shown in Figure 3.14 



Fig. 3.13 Elbow manipulator 



Fig. 3.14 Projection of the wrist center onto xo — yo plane 
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We see from this projection that 

9\ = dXaa2(x c ,y c ) (3.44) 

in which atan2(a;,y) denotes the two argument arctangent function defined in 
Chapter [2] 

Note that a second valid solution for 9\ is 


?i = 7T + atan2(a: c , y c ) 


(3.45) 


Of course this will, in turn, lead to different solutions for 62 and 63 , as we will 
see below. 

These solutions for 9i, are valid unless x c = y c = 0. In this case (3.441 is un- 
defined and the manipulator is in a singular configuration, shown in Figure |3.15| 
In this position the wrist center o c intersects Zq' hence any value of 9\ leaves o c 


A z 0 



fixed. There are thus infinitely many solutions for 9\ when o c intersects zq. 

If there is an offset d ^ 0 as shown in Figure |3.16| then the wrist center 
cannot intersect Zq. In this case, depending on how the DH parameters have 
been assigned, we will have = d or = d. In this case, there will, in general, 
be only two solutions for 9\. These correspond to the so-called left arm and 
right arm configurations as shown in Figures 3.17| and |3.18| Figure [3T7 shows 
the left arm configuration. From this figure, we see geometrically that 


9\ = <f> — a 


(3.46) 
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Fig. 3.18 Right arm configuration 


The second 
is given by 


solution, given by the right arm configuration 


shown in Figure 3.18 


0i = atan2(a: c , y c ) + atan2 sj r 2 — d 2 ,—dj (3.49) 

To see this, note that 


0i 

— QL ~\~ (3 

(3.50) 

a 

= atan2 (x c ,y c ) 

(3.51) 

(3 

= 7 + 7T 

(3.52) 

7 

= atan2(yV 2 — d 2 , d) 

(3.53) 


which together imply that 

/ 3 = atan2 (^—y/r 2 — d 2 , — dj (3.54) 


since cos(0 + n) = — cos(0) and sin(0 + tv) = — sin(0). 

To find the angles 02, 03 for the elbow manipulator, given 0i, we consider the 
plane formed by the second and third links as shown in Figure [3. 19| Since the 
motion of links two and three is planar, the solution is analogous to that of the 
two- link manipulator of Chapter [l] As in our previous derivation (cf. (1.7 1 and 
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Fig. 3.19 Projecting onto the plane formed by links 2 and 3 


(1.8)) we can apply the law of cosines to obtain 

„2 


COS 03 = 


s 2 — a 2 — a 2 


2a2(Z3 

+ y 2 - d 2 + (z c - di) 2 - a| - . 


l 3 . 


2a2®3 


= D 


(3.55) 


since r 2 = x 2 + y 2 — d 2 and s = z c — d,\. Hence, d 3 is given by 

0 3 = atan2 (p,±s/l- D 2 ^j (3.56) 

The two solutions for 0 3 correspond to the elbow-up position and elbow-down 
position, respectively. 

Similarly 02 is given as 


02 = atan2(r, s) — atan2(a 2 + CI 3 C 3 , a 3 s 3 ) (3.57) 

= atan2 ^ x% + y 2 — d 2 , z c — - atan2 (a 2 + a 3 c 3 , a 3 s 3 ) 

An example of an elbow manipulator with offsets is the PUMA shown in 
Figure |3.20| There are four solutions to the inverse position kinematics as 
shown. These correspond to the situations left arm-elbow up, left arm-elbow 
down, right arm-elbow up and right arm-elbow down. We will see that there 
are two solutions for the wrist orientation thus giving a total of eight solutions 
of the inverse kinematics for the PUMA manipulator. 


3. 3. 3. 2 Spherical Configuration 


We next solve the inverse position kinematics 

As 


for a three degree of freedom spherical manipulator shown in Figure 3.21 
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Fig. 3.20 Four solutions of the inverse position kinematics for the PUMA manipulator 
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Fig. 3.21 Spherical manipulator 


in the case of the elbow manipulator the first joint variable is the base rotation 
and a solution is given as 


9i = atan2(x c , y c ) (3.58) 

provided x c and y c are not both zero. If both x c and y c are zero, the configu- 
ration is singular as before and 9i may take on any value. As in the case of the 
elbow manipulator, a second solution for 9\ is given by 

9i = n + atan2 (x c ,y c ). (3.59) 

The angle 0 2 is given from Figure [3.21 1 as 

02 = atan2(r, s) + ^ (3.60) 

where r 2 = + y 2 , s = z c — d\. 

The linear distance cfo is found as 

d 3 = \/r 2 + s 2 = \Z x l + Vc + ( z c - di ) 2 (3.61) 

The negative square root solution for d 3 is disregarded and thus in this case 
we obtain two solutions to the inverse position kinematics as long as the wrist 
center does not intersect zq. If there is an offset then there will be left and right 
arm configurations as in the case of the elbow manipulator (Problem |3]|25| . 
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Table 3.6 Link parameters for the articulated manipulator of Figure [3. 13| 


Link 

ai 

OLi 

di 

Oi 

1 

0 

90 

d± 

9 f 

2 

0 2 

0 

0 

0*2 

3 

03 

0 

0 

0*3 


* variable 


3.3.4 Inverse Orientation 


In the previous section we used a geometric approach to solve the inverse posi- 
tion problem. This gives the values of the first three joint variables correspond- 
ing to a given position of the wrist origin. The inverse orientation problem is 
now one of finding the values of the final three joint variables corresponding to 
a given orientation with respect to the frame o 3 x 3 y 3 z 3 . For a spherical wrist, 
this can be interpreted as the problem of finding a set of Euler angles corre- 
sponding to a given rotation matrix R. Recall that equation (3.151 shows that 
the rotation matrix obtained for the spherical wrist has the same form as the 
rotation matrix for the Euler transformation, given in (2.27 1. Therefore, we can 
use the method developed in Section (2JTT] to solve for the three joint angles of 
the spherical wrist. In particular, we solve for the three Euler angles, <j>, 9, if, 
using Equations (2.29) (2.341, and then use the mapping 


9a = 4> 

9 5 = 9 

06 = i> 


Example 3.8 Articulated Manipulator with Spherical Wrist 

The DH parameters for the frame assignment shown in Figure \3.13 \ are sum- 
marized in Table \3J ) | Multiplying the corresponding matrices gives the ma- 
trix R 3 for the articulated or elbow manipulator as 


R 


o 

3 


ClC23 ^1^23 S 1 

S1C23 — S1S23 “Cl 

S 23 C23 0 


The matrix Rq = AaA^Aq is given as 


R 


3 

6 


C4C5C6 54 S6 

C4C5S6 54 Cq 

C4S5 

S4C5C6 + C4S6 

54 05^6 + C4CQ 

S4S5 

55C6 

S 5&6 

C5 


The equation to be solved for the final three variables is therefore 

Rl = (R° 3 ) t R 


(3.62) 


(3.63) 


(3.64) 
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and the Euler angle solution can be applied to this equation. For example, the 
three equations given by the third column in the above matrix equation are given 
by 


C4S5 = Cl 023^13 + S1C23C23 + S23C33 
S4S5 = ^CiS 2 3 r 13 ~ s l s 23 r 23 + c 23 r 33 

C5 = Siri3 - c±r 23 


(3.65) 

(3.66) 

(3.67) 


Hence, if not both of the expressions (3.65), (3.66) are zero, we obtain 65 from 
( IJfy and | as 


= atan2 (siri 3 - c 1 r 23 ,±\/T - (sin 3 - cir 23 ) 2 ) 


(3.68) 


and (2.32), respectively, as 


If the positive square root is chosen in (3.68), then O 4 and 9 q are given by ( 2.31 ) 


9 4 = atan2(cic 23 ri 3 + Sic 23 r23 + S 23 C 33 , 

— C1S23C13 — S1S23C23 + C23C33) 

9 6 = atan2(-s 1 r 11 + c 1 r- 2 i,sir 1 2 -c 1 r 2 2 ) 


(3.69) 

(3.70) 


The other solutions are obtained analogously. If S 3 = 0, then joint axes Z 3 and 
Z 5 are collinear. This is a singular configuration and only the sum 64 + 6 q can 
be determined. One solution is to choose 64 arbitrarily and then determine 9 q 


using (2.36) or (2.38). 
o 


3.3.5 Examples 


Example 3.9 Elbow Manipulator - Complete Solution 

To summarize the geometric approach for solving the inverse kinematics 
equations, we write give here one solution to the inverse kinematics of the six 
degree- of- freedom elbow manipulator shown in Figure \3.13\ which has no joint 
offsets and a spherical wrist. 

Given 


Ox 


rn 

T 12 

T 13 

°y 

; R = 

T 21 

C22 

r-23 

O z 


. r 31 

C32 

C33 . 


then with 


x c = 

Ox — d§r 13 

He = 

o y — d^r 23 

z c = 

o z - d 6 r 33 


(3.71) 


(3.72) 

(3.73) 

(3.74) 


z, 
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a set of DH joint variables is given by 
di 


atan2(a i c ,y c ) (3.75) 

atan2 + y% — d 2 , z c — dij — atan2(a2 + 0303, 0383) (3.76) 


0 3 = atan2 (l),±\/l- D 2 ) 


e± = 

@5 — 

^6 = 


„ fte „ D = + yg - d 3 + (^ - rfi) a - aj - ^ 

2tt2 a 3 

atan2(ciC23r 13 + SiC 23T23 + S23r 33 , 

— CiS23ri3 — SlS23r23 + C23^33) 
atan2 (siri 3 - Cir 23 , ±\/l - (sir 13 - Cir 23 ) 2 ) 
atan2(— sim + cir 2 i, siri 2 - cir 22 ) 


The other possible solutions are left as an exercise (Problem\^2I^ . 


(3.77) 

(3.78) 

(3.79) 

(3.80) 


Example 3.10 SCARA Manipulator 

As another example, we consider the SCARA manipulator whose forward 
kinematics is defined by T 4 from (3.30). The inverse kinematics solution is 
then given as the set of solutions of the equation 


rpl _ 

1 i ~ 


R o 
0 1 


C12C4 + S12S4 Sl 2 c 4 — c 12 s 4 0 a l c l + <J2 C 12 
S12C4 — C12S4 — C12C4 — S12S4 0 OlSl + 02S12 

0 0 — 1 — d 3 — d^ 

0 0 0 1 


(3.81) 

We first note that, since the SCARA has only four degrees- of- freedom, not 
every possible H from SE(3) allows a solution of (3.81). In fact we can easily 
see that there is no solution of (3.81 ) unless R is of the form 


R = 


c a s a 0 
s a c a 0 
0 0-1 


and if this is the case, the sum 6 1 + 82 — 84 is determined by 
8\ + 82 — 84 = a = atan2(r 11; r 12 ) 


(3.82) 


(3.83) 


Projecting the manipulator configuration onto the Xq — yo plane immediately 
yields the situation of Figure \3.22[ We see from this that 


9 2 = atan2 (c 2 , ±\/l - c 2 ) 


(3.84) 
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Fig. 3.22 SCARA manipulator 


where 


C2 = 


o 2 x + o 2 y -a\-al 


2a\a 2 

0 1 = atan 2 (o s , o y ) — atan 2 (ai + a 2 C 2 , a 2 S 2 ) 


We may then determine 9 4 from (f 3.83) as 


Finally d 3 is given as 


= 9i + 0 2 - a 
= 9i + 62 — atan 2 (rn,ri 2 ) 


d 3 = o z + d± 


(3.85) 

(3.86) 


(3.87) 


(3.88) 


3.4 CHAPTER SUMMARY 

In this chapter we studied the relationships between joint variables, qt and 
the position and orientation of the end effector. We begain by introducing 
the Denavit-Hartenberg convention for assigning coordinate frames to the links 
of a serial manipulator. We may summarize the procedure based on the DH 
convention in the following algorithm for deriving the forward kinematics for 
any manipulator. 

Step 1: Locate and label the joint axes Zq. ... , z n -\. 

Step 2: Establish the base frame. Set the origin anywhere on the zo-axis. The 
Xq and i/o axes are chosen conveniently to form a right-handed frame. 
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For i = 1, . . . , n — 1, perform Steps 3 to 5. 

Step 3 : Locate the origin o i where the common normal to Zi and z,_i intersects 
Zi. If Zi intersects Zi-i locate o i at this intersection. If Zi and z,_i are 
parallel, locate o i in any convenient position along z t . 

Step 4: Establish Xi along the common normal between Zj_i and z, through 
o i5 or in the direction normal to the z,;_i — Zj plane if z,_i and z, intersect. 

Step 5: Establish yi to complete a right-handed frame. 

Step 6: Establish the end-effector frame o n x n y n z n . Assuming the n-th joint 
is revolute, set z n = a along the direction z„_i- Establish the origin o n 
conveniently along z n , preferably at the center of the gripper or at the 
tip of any tool that the manipulator may be carrying. Set y n = s in the 
direction of the gripper closure and set x n = n as s x a. If the tool is 
not a simple gripper set x n and y n conveniently to form a right-handed 
frame. 

Step 7: Create a table of link parameters Oj, dj, oti, d % . 


di = distance along Xi from o i to the intersection of the Xi and z*_i axes. 

di = distance along Zj_i from o i _ 1 to the intersection of the Xi and Zi—i 
axes, di is variable if joint i is prismatic. 

ai = the angle between Zj_i and z* measured about Xi. 

di = the angle between Xi-i and Xi measured about z»_i. di is variable 
if joint i is revolute. 


Step 8: Form the homogeneous transformation matrices A, by substituting 
the above parameters into (3.101. 


Step 9: Form = A\ ■ ■ ■ A n . This then gives the position and orientation of 
the tool frame expressed in base coordinates. 


This DH convention defines the forward kinematics equations for a manipu- 
lator, i.e., the mapping from joint variables to end effector position and orienta- 
tion. To control a manipulator, it is necessary to solve the inverse problem, i.e., 
given a position and orientation for the end effector, solve for the corresponding 
set of joint variables. In this chapter, we have considered the special case of 
manipulators for which kinematic decoupling can be used (e.g., a manipulator 
with a sperical wrist). For this class of manipulators the determination of the 
inverse kinematics can be summarized by the following algorithm. 

Step 1: Find q -\ , (j 2 , <?.3 such that the wrist center o c has coordinates given by 


o 


o 

C 


0 

0 

1 


o — d 6 i? 


(3.89) 
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Step 2: Using the joint variables determined in Step 1, evaluate R 3 . 

Step 3 : Find a set of Euler angles corresponding to the rotation matrix 

Rl = (R° 3 )- 1 R = (R° 3 ) t R (3.90) 

In this chapter, we demonstrated a geometric approach for Step 1. In par- 
ticular, to solve for joint variable qt, we project the manipulator (including the 
wrist center) onto the — yi-\ plane and use trigonometry to find q j. 


3.5 NOTES AND REFERENCES 

Kinematics and inverse kinematics have been the subject of research in robotics 
for many years. Some of the seminal work in these areas can be found in [5] 

[m uni i^] B 3 ] m] inni d] es [znj ej 133 eh mi ns uni m] eb] izg. 
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Problems 


1. Verify the statement after Equation (3.141 that the rotation matrix R has 
the form (3.131 provided assumptions DH1 and DH2 are satisfied. 

2. Consider the three-link planar manipulator shown in Figure [3. 23| Derive 



Fig. 3.23 Three-link planar arm of Problem |3|2| 

the forward kinematic equations using the DH-convention. 

3. Consider the two- link cartesian manipulator of Figure |3.24| Derive the 



Fig. 3.24 Two-link cartesian robot of Problem |3|3 
forward kinematic equations using the DH-convention. 


4. Consider the two- link manipulator of Figure 3.25 which has joint 1 revo- 
lute and joint 2 prismatic. Derive the forward kinematic equations using 
the DH-convention. 


5. Consider the three-link planar manipulator of Figure 3.26 Derive the for- 
ward kinematic equations using the DH-convention. 


6. Consider the three-link articulated robot of Figure 3.27 Derive the for- 
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Fig. 3.25 Two-link planar arm of Problem |3|4| 



Fig. 3.26 Three-link planar arm with prismatic joint of Problem |3| 5 1 



ward kinematic equations using the DH-convention. 

7. Consider the three-link cartesian manipulator of Figure |3.28| Derive the 
forward kinematic equations using the DH-convention. 


Attach a spherical wrist to the three-link articulated manipulator of Prob- 
lem [3|6] as shown in Figure |3.29| 


Derive the forward kinematic equations 


for this manipulator. 
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Fig. 3.28 Three-link cartesian robot 

A 



Ul 


Fig. 3.29 Elbow manipulator with spherical wrist 


9. 


10 . 


Attach a spherical wrist to the three-link cartesian manipulator of Prob- 
lem 3][7 as shown in Figure 3.30 Derive the forward kinematic equations 
for this manipulator. 


Consider the PUMA 260 manipulator shown in Figure |3.31| Derive the 
complete set of forward kinematic equations, by establishing appropriate 
DH coordinate frames, constructing a table of link parameters, forming 
the A-matrices, etc. 

11. Repeat Problem ffl for the five degree-of-freedom Rhino XR-3 robot 

(Note: you should replace the Rhino wrist with 


shown in Figure 3.32 
the sperical wrist.) 

12. Suppose that a Rhino XR-3 is bolted to a table upon which a coordi- 
nate frame o s x s y s z s is established as shown in Figure 3.33 (The frame 
o s x s y x z s is often referred to as the station frame.) Given the base frame 
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Fig. 3.30 Cartesian manipulator with spherical wrist 


that you established in Problem 3fTT find the homogeneous transforma- 
tion Tq relating the base frame to the station frame. Find the homo- 
geneous transformation T| relating the end-effector frame to the station 
frame. What is the position and orientation of the end-effector in the 
station frame when 6 \ = 62 = ■ ■ • = O 5 = 0? 


13. Consider the GMF S-400 robot shown in Figure |3.34| Draw the symbolic 
representation for this manipulator. Establish DH-coordinate frames and 
write the forward kinematic equations. 


14. Given a desired position of the end-effector, how many solutions are 
there to the inverse kinematics of the three-link planar arm shown in 


Figure 3.35? If the orientation of the end-effector is also specified, how 
many solutions are there? Use the geometric approach to find them. 


15. Repeat Problem 3fl4 for the three-link planar arm with prismatic joint 
of Figure |3.36| 


16. Solve the inverse position kinematics for the cylindrical manipulator of 
Figure |3.37| 


17. Solve the inverse position kinematics for the cartesian manipulator of 
Figure |3.38| 
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Fig. 3.31 PUMA 260 manipulator 
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Fig. 3.32 Rhino XR-3 robot 


18. Add a spherical wrist to the three-link cylindrical arm of Problem 3|[T6 
and write the complete inverse kinematics solution. 

19. Repeat Problem [3|T6| for the cartesian manipulator of Problem [3|T7| 


20. Write a computer program to compute the inverse kinematic equations for 


the elbow manipulator using Equations (3.75 >- ( 3.80 ) . Include procedures 


for identifying singular configurations and choosing a particular solution 
when the configuration is singular. Test your routine for various special 
cases, including singular configurations. 


21. The Stanford manipulator of Example 3.3.5 has a spherical wrist. There- 
fore, given a desired position O and orientation R of the end-effector, 


a) Compute the desired coordinates of the wrist center O® . 

b) Solve the inverse position kinematics, that is, find values of the first 

three joint variables that will place the wrist center at O c . Is the 
solution unique? How many solutions did you find? 
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Fig. 3.33 Rhino robot attached to a table. From: A Robot Engineering Textbook, by 
Mohsen Shahinpoor. Copyright 1987, Harper & Row Publishers, Inc 
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S-400 

DIMENSIONS 



0 


O 




Fig. 3.34 GMF S-400 robot. (Courtesy GMF Robotics.) 
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Fig. 3.35 Three-link planar robot with revolute joints. 



c) Compute the rotation matrix Solve the inverse orientation prob- 
lem for this manipulator by finding a set of Euler angles correspond- 
ing to Rq given by (3.63 1 . 


22. Repeat Problem 3f2l for the PUMA 260 manipulator of Problem 3]|9 


which also has a spherical wrist. How many total solutions did you find? 

23. Solve the inverse position kinematics for the Rhino robot. 

24. ). Find all other solutions to the inverse kinematics of the elbow manip- 


ulator of Example 3.9 


25. . Modify the solutions 0\ and 02 for the spherical manipulator given by 
Equations (3.581 and (3.60) in the case of a shoulder offset. 




VELOCITY 

KINEMATICS - THE 
MANIPULATOR 
JACOBIAN 

In the previous chapter we derived the forward and inverse position equations 
relating joint positions to end-effector positions and orientations. In this chapter 
we derive the velocity relationships, relating the linear and angular velocities of 
the end-effector to the joint velocities. 

Mathematically, the forward kinematic equations define a function between 
the space of cartesian positions and orientations and the space of joint posi- 
tions. The velocity relationships are then determined by the Jacobian of this 
function. The Jacobian is a matrix that can be thought of as the vector version 
of the ordinary derivative of a scalar function. The Jacobian is one of the most 
important quantities in the analysis and control of robot motion. It arises in 
virtually every aspect of robotic manipulation: in the planning and execution 
of smooth trajectories, in the determination of singular configurations, in the 
execution of coordinated anthropomorphic motion, in the derivation of the dy- 
namic equations of motion, and in the transformation of forces and torques 
from the end-effector to the manipulator joints. 

We begin this chapter with an investigation of velocities, and how to rep- 
resent them. We first consider angular velocity about a fixed axis, and then 
generalize this to rotation about an arbitrary, possibly moving axis with the 
aid of skew symmetric matrices. Equipped with this general representation of 
angular velocities, we are able to derive equations for both the angular velocity 
and the linear velocity for the origin of a moving frame. 

We then proceed to the derivation of the manipulator Jacobian. For an ?r-link 
manipulator we first derive the Jacobian representing the instantaneous trans- 
formation between the n-vector of joint velocities and the 6- vector consisting 
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of the linear and angular velocities of the end-effector. This Jacobian is then 
a 6 x n matrix. The same approach is used to determine the transformation 
between the joint velocities and the linear and angular velocity of any point 
on the manipulator. This will be important when we discuss the derivation of 
the dynamic equations of motion in Chapter [bj We then discuss the notion of 
singular configurations. These are configurations in which the manipulator 
loses one or more degrees-of- freedom. We show how the singular configurations 
are determined geometrically and give several examples. Following this, we 
briefly discuss the inverse problems of determining joint velocities and accelera- 
tions for specified end-effector velocities and accelerations. We end the chapter 
by considering redundant manipulators. This includes discussions of the inverse 
velocity problem, singular value decomposition and manipulability. 


4.1 ANGULAR VELOCITY: THE FIXED AXIS CASE 

When a rigid body moves in a pure rotation about a fixed axis, every point 
of the body moves in a circle. The centers of these circles lie on the axis of 
rotation. As the body rotates, a perpendicular from any point of the body to 
the axis sweeps out an angle 9 , and this angle is the same for every point of 
the body. If k is a unit vector in the direction of the axis of rotation, then the 
angular velocity is given by 

u> = 9k (4.1) 

in which 9 is the time derivative of 9. 

Given the angular velocity of the body, one learns in introductory dynamics 
courses that the linear velocity of any point on the body is given by the equation 

v = u i x r (4.2) 

in which r is a vector from the origin (which in this case is assumed to lie on 
the axis of rotation) to the point. In fact, the computation of this velocity v 
is normally the goal in introductory dynamics courses, and therefore, the main 
role of an angular velocity is to induce linear velocities of points in a rigid body. 
In our applications, we are interested in describing the motion of a moving 
frame, including the motion of the origin of the frame through space and also 
the rotational motion of the frame’s axes. Therefore, for our purposes, the 
angular velocity will hold equal status with linear velocity. 

As in previous chapters, in order to specify the orientation of a rigid object, 
we attach a coordinate frame rigidly to the object, and then specify the orien- 
tation of the attached frame. Since every point on the object experiences the 
same angular velocity (each point sweeps out the same angle 9 in a given time 
interval) , and since each point of the body is in a fixed geometric relationship to 
the body-attached frame, we see that the angular velocity is a property of the 
attached coordinate frame itself. Angular velocity is not a property of individ- 
ual points. Individual points may experience a linear velocity that is induced 
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by an angular velocity, but it makes no sense to speak of a point itself rotating. 
Thus, in Equation (4.2 I v corresponds to the linear velocity of a point, while u> 
corresponds to the angular velocity associated with a rotating coordinate frame. 

In this fixed axis case, the problem of specifying angular displacements is 
really a planar problem, since each point traces out a circle, and since every 
circle lies in a plane. Therefore, it is tempting to use 6 to represent the angular 
velocity. However, as we have already seen in Chapter [2j this choice does not 
generalize to the three-dimensional case, either when the axis of rotation is not 
fixed, or when the angular velocity is the result of multiple rotations about 
distinct axes. For this reason, we will develop a more general representation for 
angular velocities. This is analogous to our development of rotation matrices in 
Chapter [2] to represent orientation in three dimensions. The key tool that we 
will need to develop this representation is the skew symmetric matrix, which is 
the topic of the next section. 


4.2 SKEW SYMMETRIC MATRICES 


In Section |4.3| we will derive properties of rotation matrices that can be used 
to compute relative velocity transformations between coordinate frames. Such 
transformations involve derivatives of rotation matrices. By introducing the 
notion of a skew symmetric matrix it is possible to simplify many of the com- 
putations involved. 


Definition 4.1 An nx n matrix S is said to be skew symmetric if and only 

if 

S T + S = 0 (4.3) 


We denote the set of all 3 x 3 skew symmetric matrices by so(3). If S £ so( 3) 
has components Sij, i,j = 1,2,3 then Equation (4.3 1 is equivalent to the nine 
equations 


+ S ji 


= 0 


i,j = 1,2,3 


(4.4) 


From Equation (4.4) we see that su = 0; that is, the diagonal terms of S are 
zero and the off diagonal terms s. t j , i j satisfy s*j = — Sji. Thus S contains 
only three independent entries and every 3x3 skew symmetric matrix has the 
form 


S 


0 -s 3 s 2 

s 3 0 -Si 
— s 2 Si 0 


(4.5) 


If a = ( a x , a y ,a z ) T is a 3- vector, we define the skew symmetric matrix S(a) as 


S{a) 


0 CL Z CLy 

CL Z 0 CL X 

CL y CL X 0 
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Example 4.1 

We denote by i, j and k the three unit basis coordinate vectors, 


i = 

' i ' 
0 

; j = 

' o ' 
i 

; k = 

1 

0 o 

1 


0 


0 


1 


The skew symmetric matrices S(i), S(j), and S(k) are given by 


S{i) 

S(k) 


0 0 0 

0 0-1 

0 1 0 

0-10 
1 0 0 

0 0 0 


S(j) 


0 0 1 

0 0 0 

-10 0 


o 


4.2.1 Properties of Skew Symmetric Matrices 

Skew symmetric matrices possess several properties that will prove useful for 
subsequent derivations 0 Among these properties are 

1. The operator S is linear, i.e., 


S(aa + /3b) = aS(a) + /3S(b) (4-6) 

for any vectors a and b belonging to R 3 and scalars a and (3. 

2. For any vectors a and p belonging to R 3 , 

S(a)p = ax p (4.7) 


where a x p denotes the vector cross product. Equation (4.7 1 can be 
verified by direct calculation. 

3. If R £ 5*0(3) and a, b are vectors in R 3 it can also be shown by direct 
calculation that 


R(a x b) = Ra x Rb 


(4.8) 


Equation (4.8 1 is not true in general unless R is orthogonal. Equa- 
tion (4.8 1 says that if we first rotate the vectors a and b using the rotation 
transformation R and then form the cross product of the rotated vectors 


1 These properties are consequences of the fact that so(3) is a Lie Algebra , a vector space with 
a suitably defined product operation [5] . 
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Ra and Rb 1 the result is the same as that obtained by first forming the 
cross product a x b and then rotating to obtain R(a x b). 

4. For R e 50(3) and a£l 3 

RS(a)R T = S(Ra) (4.9) 


This property follows easily from Equations (4.7) and (4.8) as follows. Let 
b £ R 3 be an arbitrary vector. Then 


RS(a)R T b 


R(a x R T b ) 
(Ra) x (RR T b) 
(Ra) x b 
S(Ra)b 


and the result follows. 


As we will see, Equation (|4.9| is one of the most useful expressions that we 


will derive. The left hand side of Equation (4.9) represents a similarity trans- 
formation of the matrix S(a). The equation says therefore that the matrix 
representation of S(a) in a coordinate frame rotated by R is the same as the 
skew symmetric matrix S(Ra) corresponding to the vector a rotated by R. 


4.2.2 The Derivative of a Rotation Matrix 

Suppose now that a rotation matrix R is a function of the single variable 9. 
Hence R = R(9) £ 50(3) for every 8. Since R is orthogonal for all 9 it follows 
that 


R(8)R(8) t = I 


(4.10) 


Differentiating both sides of Equation (4.10 ) with respect to 9 using the product 
rule gives 

d l RWT + ‘ 

Let us define the matrix 5 as 


d8 


Then the transpose of 5 is 


dR 

~dd J 


Equation (4.11) says therefore that 


dR T 

a- » 

< 

II 

o 

(4.11) 

]r(8) t 

(4.12) 

\ T dU T 

) = m d9 

(4.13) 

= 0 

(4.14) 
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In other words, the matrix S defined by Equation (4.12) is skew symmetric. 


Multiplying both sides of Equation (4.12 1 on the right by R and using the fact 
that r! R = / yields 

dR 

hd) 


= SR{9) 


(4.15) 


Equation (4.151 is very important. It says that computing the derivative 
of the rotation matrix R is equivalent to a matrix multiplication by a skew 
symmetric matrix S. The most commonly encountered situation is the case 
where R is a basic rotation matrix or a product of basic rotation matrices. 


Example 4.2 

If R = R x g, the basic rotation matrix given by Equation (2.6), then direct 
computation shows that 


dR T 

s ~le R ~ 


0 0 0 
0 — sin 6 — cos 6 

0 cos 9 — sin 9 



' 1 

0 

0 


0 

cos 9 

sin(? 


0 

— sin# 

COS0 


0 0 0 

0 0-1 

0 1 0 


= S(i) 


Thus we have shown that 


dR . „ 


d9 


= S(i)R x 


Similar computations show that 


dR 


v,s 


d9 


= S(j)R y9 and 


dR z ,i 

de 


— S(k)R z g 


(4.16) 


Example 4.3 

Let R k e be a rotation about the axis defined by k as in Equation (2.^6). Note 
that in this example k is not the unit coordinate vector (0,0, 1) T . It is easy to 
check that S(k) 3 = —S(k). Using this fact together with Problem ~4§25 it follows 
that 


dR. 


k,e 


de 


— S(k)R ke 


(4.17) 


4.3 ANGULAR VELOCITY: THE GENERAL CASE 

We now consider the general case of angular velocity about an arbitrary, pos- 
sibly moving, axis. Suppose that a rotation matrix R is time varying, so that 
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R = R(t) £ SO( 3) for every t £ R. Assuming that R(t) is continuously dif- 
ferentiable as a function of t, an argument identical to the one in the previous 
section shows that the time derivative R(t) of R(t) is given by 

R(t) = S(t)R(t) (4.18) 

where the matrix S(t) is skew symmetric. Now, since S(t) is skew symmetric, 
it can be represented as S(u>(t)) for a unique vector This vector u(t) is 

the angular velocity of the rotating frame with respect to the fixed frame at 
time t. Thus, the time derivative R(t) is given by 

R(t) = S(u>(t))R(t) (4.19) 


in which ui(t) is the angular velocity. 

Equation ( |4.19 I shows the relationship between angular velocity and the 
derivative of a rotation matrix. In particular, if the instantaneous orientation 
of a frame OiX\yiZ\ with respect to a frame OqXqUqZq is given by R\, then the 
angular velocity of frame o\Xiy\Zi is directly related to the derivative of R\ 
by Equation (4.19). When there is a possibility of ambiguity, we will use the 
notation coij to denote the angular velocity that corresponds to the derivative of 
the rotation matrix RL. Since w is a free vector, we can express it with respect 
to any coordinate system of our choosing. As usual we use a superscript to 
denote the reference frame. For example, 2 would give the angular velocity 
that corresponds to the derivative of R\ 1 expressed in coordinates relative to 
frame ooxoyozo- In cases where the angular velocities specify rotation relative to 
the base frame, we will often simplify the subscript, e.g., using u >2 to represent 
the angular velocity that corresponds to the derivative of i? 2 . 


Example 4.4 

Suppose that R(t) — R x . Then R(t) is computed using the chain rule as 
k=d ^ = d § , f t = SS(i)R(t) = S(u,(t))R{t) (4.20) 


in which oj = id is the angular velocity. Note, here i = (1,0, 0) T . 


4.4 ADDITION OF ANGULAR VELOCITIES 

We are often interested in finding the resultant angular velocity due to the 
relative rotation of several coordinate frames. We now derive the expressions 
for the composition of angular velocities of two moving frames OiXiyiZi and 
02 X 2 P 2 Z 2 relative to the fixed frame ooxoyozo ■ For now, we assume that the 
three frames share a common origin. Let the relative orientations of the frames 
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oiXiyiZi and 02 ^ 22 / 2-2 be given by the rotation matrices Ri{t) and R\(t) (both 
time varying) . As in Chapter [ 2 ] 

R° 2 {t) = Rlmlif) (4-21) 


Taking derivatives of both sides of Equation (4.21 1 with respect to time yields 

RO = ro r i + R o R i ( 422 ) 


Using Equation (4.191, the term R 2 on the left-hand side of Equation (4.221 
can be written 


R°2 


= SU 2 )Ri_ 


(4.23) 


In this expression, u>q 2 denotes the total angular velocity experienced by frame 
02 X 2 V 2 Z 2 - This angular velocity results from the combined rotations expressed 
by Ri and R\. 


The first term on the right-hand side of Equation (4.22) is simply 


R\R\ = SfuS.MRa = SivS'jRg 


(4.24) 


Note that in this equation, denotes the angular velocity of frame 0 \X\yiZi 
that results from the changing and this angular velocity vector is expressed 
relative to the coordinate system ogXgygZg. 

Let us examine the second term on the right hand side of Equation (4.221. 
Using Equation (4.9) we have 


RlS{u>l 2 )R\ 

(4.25) 

R\S(ul 2 )Rf RlRl = S{Rlul 2 )R\Rl 


5(i?Vi i2 )i?°. 

(4.26) 


Note that in this equation, ui\ 2 denotes the angular velocity of frame 02 X 22 / 2^2 
that corresponds to the changing R\ , expressed relative to the coordinate system 
OiXiyiZi. Thus, the product Ri(x{ 2 expresses this angular velocity relative to 
the coordinate system ooXoyoZo, he., RiXi{ 2 gives the coordinates of the free 
vector u>i t 2 with respect to frame 0. 

Now, combining the above expressions we have shown that 


s(lo° 2 )r° 2 = {SK° ,) + 


Since S(a ) + S(b) = S(a + b), we see that 


uj 2 


= oj, 


0,1 




(4.27) 


(4.28) 


In other words, the angular velocities can be added once they are expressed 
relative to the same coordinate frame, in this case oqXoJ/oA)- 
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The above reasoning can be extended to any number of coordinate systems. 
In particular, suppose that we are given 

R°n = RlRl-'-R r 1 (4-29) 

Although it is a slight abuse of notation, let us represent by u the an- 
gular velocity due to the rotation given by R l ~ , expressed relative to frame 
Oi-iXi-iyt-iZi-i. Extending the above reasoning we obtain 

R°n = S(cj° 0in )R° n (4.30) 

in which 

w 0 ,n = w 0,l + ^l w l,2 + ^2 W 2,3 + ^3 W 3,4 4 b (4-31) 

= + W° 2 + w 2,3 + W 3,4 H b w n-l,n (4.32) 


4.5 LINEAR VELOCITY OF A POINT ATTACHED TO A MOVING 
FRAME 


We now consider the linear velocity of a point that is rigidly attached to a mov- 
ing frame. Suppose the point p is rigidly attached to the frame OiXiyiZi, and 
that OiXiyiZi is rotating relative to the frame OqXqi/qZq. Then the coordinates 
of p with respect to the frame OqXqDqZq are given by 

P° = R\(t)p\ (4.33) 

The velocity p° is then given by the product rule for differentiation as 

p° = R^p 1 + R^p 1 (4.34) 

= SV 0 )!??^ 1 (4.35) 

= S(u>°)p° = u>° xp° (4.36) 


which is the familiar expression for the velocity in terms of the vector cross 
product. Note that Equation (4.351 follows from that fact that p is rigidly 


attached to frame OiXiyiZi, and therefore its coordinates relative to frame 
OiXiyiZi do not change, giving p 1 = 0. 

Now suppose that the motion of the frame OiXiyiZi relative to OoXoyoZo is 
more general. Suppose that the homogeneous transformation relating the two 
frames is time-dependent, so that 


H°i{t) = 


R°i(t) o?(t) 


0 


1 


(4.37) 


For simplicity we omit the argument t and the subscripts and superscripts 
on i?)* and and write 


p = 


Rp 1 + o 


(4.38) 
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Differentiating the above expression using the product rule gives 

p° = Rp 1 + 6 (4.39) 

= S(u>)Rp 1 + o 
= lo x r + v 

where r = Rp 1 is the vector from o\ to p expressed in the orientation of the 
frame ooXoyoZo, and v is the rate at which the origin oi is moving. 

If the point p is moving relative to the frame oiXiyiZi, then we must add to 
the term v the term R{t)p l , which is the rate of change of the coordinates p 1 
expressed in the frame ooXoyoZQ. 


4.6 DERIVATION OF THE JACOBIAN 


Consider an n-link manipulator with joint variables q ±, . . . , q n . Let 


R°n(q) °n(q) 

o 1 


(4.40) 


denote the transformation from the end-effector frame to the base frame, where 
q = (qi, . . . , q n ) T is the vector of joint variables. As the robot moves about, 
both the joint variables qi and the end-effector position and orientation R° 
will be functions of time. The objective of this section is to relate the linear and 
angular velocity of the end-effector to the vector of joint velocities q(t). Let 

S(u 0 n ) = R°(R°) t (4.41) 


define the angular velocity vector of the end-effector, and let 


v 


o 

n 


o 


0 

n 


(4.42) 


denote the linear velocity of the end effector. We seek expressions of the form 


v° n = J v q (4.43) 

= Juq (4.44) 


where J v and are 3xn matrices. We may write Equations (4.43 ) and (4.441 
together as 


£ = Jq 


(4.45) 


in which £ and J are given by 


£ 


and 


Jv 

J uj 


(4.46) 
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The vector £ is sometimes called a body velocity. Note that this velocity vector 
is not the derivative of a position variable, since the angular velocity vector is 
not the derivative of any particular time varying quantity. The matrix J is 
called the Manipulator Jacobian or Jacobian for short. Note that J is a 
6 x n matrix where n is the number of links. We next derive a simple expression 
for the Jacobian of any manipulator. 


4.6.1 Angular Velocity 


Recall from Equation (4.31 1 that angular velocities can be added as free vectors, 
provided that they are expressed relative to a common coordinate frame. Thus 
we can determine the angular velocity of the end-effector relative to the base 
by expressing the angular velocity contributed by each joint in the orientation 
of the base frame and then summing these. 

If the i-th joint is revolute, then the i - th joint variable qt equals 9i and the 
axis of rotation is Zi-\. Following the convention that we introduced above, let 
w.p 1 represent the angular velocity of link i that is imparted by the rotation of 
joint i, expressed relative to frame Oi-\Xi-\yi-\Zi-\. This angular velocity is 
expressed in the frame i — 1 by 

(4.47) 


wr 1 


= Qi4-i 


= qik 

in which, as above, k is the unit coordinate vector (0,0, 1) T . 

If the i-th joint is prismatic, then the motion of frame i relative to frame 
i — 1 is a translation and 

wj" 1 = 0 (4.48) 

Thus, if joint i is prismatic, the angular velocity of the end-effector does not 
depend on qi, which now equals di. 

Therefore, the overall angular velocity of the end-effector, in the base 
frame is determined by Equation (4.31) as 
, ,o 


= pxqxk + p- 2 q 2 Rik -\ 1 - p n q n R n -ik 


(4.49) 


^PiQiZi - 1 
2-1 


in which pi is equal to 1 if joint i is revolute and 0 if joint i is prismatic, since 

*°-i = Rhk (4.50) 

Of course z° = k = (0, 0, 1) T . 


The lower half of the Jacobian J u , in Equation (4.461 is thus given as 

Ju> = [PlZQ ■ ■ ■ p n Z n - 1] ■ (4-51) 


Note that in this equation, we have omitted the superscripts for the unit vectors 
along the z-axes, since these are all referenced to the world frame. In the 
remainder of the chapter we occasionally will follow this convention when there 
is no ambiguity concerning the reference frame. 
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4.6.2 Linear Velocity 

The linear velocity of the end-effector is just 6° . By the chain rule for differen- 
tiation 


o 


o 

n 


E 



( 4 . 52 ) 


Thus we see that the i-tli column of J v , which we denote as J Vi is given by 


Jvi — 


d_A 

dqi 


( 4 . 53 ) 


Furthermore this expression is just the linear velocity of the end-effector that 
would result if <ji were equal to one and the other qj were zero. In other words, 
the i-th column of the Jacobian can be generated by holding all joints fixed but 
the *-th and actuating the i-th at unit velocity. We now consider the two cases 
(prismatic and revolute joints) separately. 

(i) Case 1: Prismatic Joints 

If joint i is prismatic, then it imparts a pure translation to the end-effector. 
From our study of the DH convention in Chapter [3j we can write the as the 
product of three transformations as follows 


r ro 0 ° i 

± L n u n 

o 1 


rp U 


rp 0 rpi—1 rpi 

1 n 

Rt 
o 


o 0 

1 °%— 1 


r>^ — 1 1 

R*i °i 


' Rn °n ' 

1 


0 1 


0 1 

+ RU ot 1 + oh 

' 



( 4 . 54 ) 

( 4 . 55 ) 

( 4 . 56 ) 

( 4 . 57 ) 


which gives 


= RC,o 


0 „i 


m 


1 O: 


oti 


( 4 . 58 ) 


If only joint i is allowed to move, then both of o l n and o^ ) _ 1 are constant. 
Furthermore, if joint i is prismatic, then the rotation matrix is also con- 
stant (again, assuming that only joint i is allowed to move). Finally, recall 
from Chapter [ 3 ] that, by the DH convention, o l ~ x = (a,iCi,aiSi,di) T . Thus, 
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differentiation of o° gives 


dqt 


_ rj- 1 

dd^ 1 - 1 1 


77 0 


= diR^i 


- d-7° 


CLiCi 

O'iSi 


(4.59) 

(4.60) 

(4.61) 

(4.62) 


in which di is the joint variable for prismatic joint i. Thus, (again, dropping 
the zero superscript on the z-axis) for the case of prismatic joints we have 


JVi — Zi - 1 


(4.63) 


(ii) Case 2: Revolute Joints 


If joint i is revolute, then we have qi = 9 i. Starting with Equation (4.581, and 
letting q L = 9i, since R® is not constant with respect to 9i , we obtain 


^f n = ^[R^ + RUol 1 ] (4.64) 

= ^-R^ + Rl^ot 1 (4.65) 

= OiSizlJR^ + OiSizlJRliOr 1 (4.66) 

= OiSizh) [R°c? n + Rhot 1 ] (4.67) 

= OiSizlM-oU) (4.68) 

= BiZ °_ i x (o° - o?_i) (4.69) 

The second term in Equation ( 4.66| ) is derived as follows: 

a d%C% diSi 

R°- iww- aiSi = R°_ 1 diCi di (4.70) 

d0i [ di \ [ 0 

= RUsm)^ 1 (4-71) 

= RUS{k6i) {RU) T RU^ 1 (4-72) 

= SiRUk^RUot 1 (4.73) 

= ^SizlJRl lOp 1 (4.74) 


Equation (4.71) follows by straightforward computation, 
joint 


Thus for a revolute 


d Vi — Zi — i X (o n Oi — i) 


(4-75) 
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in which we have, following our convention, omitted the zero superscripts. Fig- 
ure 4.1 illustrates a second interpretation of Equation (4.751. As can be seen 


in the figure, o n — Oi-i = r and Zi-\ = u> in the familiar expression v = lo x r. 



Fig. 4.1 Motion of the end-effector due to link i. 


4.6.3 Combining the Angular and Linear Jacobians 


As we have seen in the preceding section, the upper half of the Jacobian J v is 
given as 


J v — [Jy i ’ • ' Jv n \ 


where the i-th column J Vi is 
Jvi = 


Zj_i x (o n — Oi_ i) for revolute joint i 
2 j_ i for prismatic joint i 


The lower half of the Jacobian is given as 

J LJ [^Jl ' ' ' J^n ] 


where the i-th column J Ui is 


J Zi-i for revolute joint i 
) 0 for prismatic joint i 


(4.76) 


(4.77) 


(4.78) 


(4.79) 


Putting the upper and lower halves of the Jacobian together, we the Jacobian 
for an n-link manipulator is of the form 


J — '1 1 J‘2 * * * Jn ] 


(4.80) 


EXAMPLES 127 


where the i-th column Ji is given by 

_ [ Zi-l X (o n ^ Oi- 1) 

Ji — 

%i— 1 

if joint i is revolute and 


Ji = 


Zi-l 

0 


(4.81) 


(4.82) 


if joint i is prismatic. 

The above formulas make the determination of the Jacobian of any manip- 
ulator simple since all of the quantities needed are available once the forward 
kinematics are worked out. Indeed the only quantities needed to compute the 
Jacobian are the unit vectors Z; and the coordinates of the origins oi, . . . , o n . A 
moment’s reflection shows that the coordinates for z% with respect to the base 
frame are given by the first three elements in the third column of T? while Oi 
is given by the first three elements of the fourth column of if. Thus only the 
third and fourth columns of the T matrices are needed in order to evaluate the 
Jacobian according to the above formulas. 

The above procedure works not only for computing the velocity of the end- 
effector but also for computing the velocity of any point on the manipulator. 
This will be important in Chapter [6] when we will need to compute the velocity 
of the center of mass of the various links in order to derive the dynamic equations 
of motion. 


4.7 EXAMPLES 


We now provide a few examples to illustrate the derivation of the manipulator 
Jacobian. 


Example 4.5 Two-Link Planar Manipulator 

Consider the two-link planar manipulator of Example \3.1\ Since both joints 
are revolute the Jacobian matrix, which in this case is 6 x 2, is of the form 


J(q) 


Z 0 X (o 2 - Oo) Z 1 X (o 2 - oi) 
Z 0 Z 1 


The various quantities above are easily seen to be 


' 0 ' 


aici 


aici + a 2 ci2 

0 

0 1 = 

aisi 

o 2 = 

aisi + a 2 si2 

0 


0 


0 


(4.83) 


(4.84) 


0 

0 

1 


z 0 = z x 


(4.85) 
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Performing the required calculations then yields 


J 


—a\Si — CI2S12 — CI2S12 
asici + a 2 ci2 a 2 ci2 
0 0 

0 0 

0 0 

1 1 


(4.86) 


It is easy to see how the above Jacobian compares with Equation (1.1 ) derived 
in Chapter [ 7 ] The first two rows of Equation (4-85) are exactly the 2x2 
Jacobian of Chapter [ 7 ] and give the linear velocity of the origin 02 relative to 
the base. The third row in Equation (4-86) is the linear velocity in the direction 
of zq, which is of course always zero in this case. The last three rows represent 
the angular velocity of the final frame, which is simply a rotation about the 
vertical axis at the rate 9i + 62 ■ 
o 


Example 4.6 Jacobian for an Arbitrary Point 

Consider the three-link planar manipulator of Figure \ 4-2\ Suppose we wish 



Fig. 4.2 Finding the velocity of link 2 of a 3-link planar robot. 


to compute the linear velocity v and the angular velocity to of the center of link 2 
as shown. In this case we have that 


J(q) 


zo x (o c - o 0 ) Z\ x (o c - 01) 0 

z 0 z 1 0 


(4.87) 


which is merely the usual the Jacobian with o c in place of o n . Note that the 
third column of the Jacobin is zero, since the velocity of the second link is 
unaffected by motion of the third linl^ Note that in this case the vector o c 
must be computed as it is not given directly by the T matrices ( Problem 13). 


2 Note that we are treating only kinematic effects here. Reaction forces on link 2 due to the 
motion of link 3 will influence the motion of link 2. These dynamic effects are treated by the 
methods of Chapter |6] 
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Example 4.7 Stanford Manipulator 

Consider the Stanford manipulator of Example \3.5\ with its associated Denavit- 
Hartenherg coordinate frames. Note that joint 3 is prismatic and that 03 = 04 = 
o 5 as a consequence of the spherical wrist and the frame assignment. Denoting 
this common origin by o we see that the columns of the Jacobian have the form 


Ji 


Zi-1 X (o 6 - Oi- 1 ) 
Zi - 1 


i = 1,2 


J 3 = 


Z2 

0 


Ji 


Zi- 1 X (o 6 - o) 
Zi-1 


i = 4,5,6 


Now, using the A-matrices given by Equations ( 3.1S)-(3.23 ) and the T- 
matrices formed as products of the A-matrices, these quantities are easily com- 
puted as follows: First, Oj is given by the first three entries of the last column 
ofTj = Ai ■ ■ ■ Aj, with oq = (0,0, 0) T = 0 \. The vector Zj is given as 


Zj = R®k 


(4.88) 


where R ° is the rotational part of T)° . Thus it is only necessary to compute 
the matrices T® to calculate the Jacobian. Carrying out these calculations one 
obtains the following expressions for the Stanford manipulator: 


06 


03 


ClS2G?3 — S\d 2 + G?6 (CiC 2C4S5 + C1C5S2 — S1S4S5) 
5152^3 ~ Cl G?2 + (ie(ciS4S5 + C2C4S1S5 + C5S1S2) 
C2C?3 + de(c 2 C 3 — C4S2S5) 


C!S 2 d 3 - Sid 2 

sis 2 d 3 + cid 2 
c 2 d 3 


(4.89) 

(4.90) 
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The 


Zi are given as 


z 4 = 


Z5 = 



' 0 ' 


-si 

z 0 = 

0 

Zl = 

Cl 


1 


0 




L J 



cis 2 


ClS 2 

22 = 

sis 2 

Z 3 = 

SlS 2 


C2 


C2 


— C 1 C 2 S 4 — S 1 C 4 
— S1C2S4 4 - C1C4 
S 2 S 4 

C1C2C4S5 — S1S4S5 + C4S2C5 
S1C2C4S5 + C1S4S5 + S1S2C5 
— S 2 C 4 S 5 + C 2 C 5 


(4.91) 


(4.92) 


(4.93) 


(4.94) 


The Jacobian of the Stanford Manipulator is now given by combining these 
expressions according to the given formulae (Problem pplfy. 


Example 4.8 SCARA Manipulator 

We will now derive the Jacobian of the SCARA manipulator of Example \3.0\ 
This Jacobian is a 6x4 matrix since the SCARA has only four degrees-of- 
freedom. As before we need only compute the matrices Tj = A\...Aj, where 
the A-matrices are given by Equations (3.26)- \3.29 ). 

Since joints 1,2, and 4 o,re revolute and joint 3 is prismatic, and since 04 — 03 
is parallel to Z 3 (and thus, Z 3 x (04 — 03 ) =0^, the Jacobian is of the form 


J = 


Z 0 X (04 — Oo) Z\ X (04 — Oi) Z2 0 
z 0 Zi 0 z 3 

Performing the indicated calculations, one obtains 



aici 


aici + a 2 c \ 2 

Ol = 

aisi 

o 2 = 

aisi + a 2 S \ 2 


0 


0 


04 = 


a 1 C 1 + a 2 c 12 
ais 2 + a 2 si2 
d 3 - d 4 


(4.95) 


(4.96) 


(4.97) 


Similarly Zo = Zi = k, and z 2 = Z 3 = —k. Therefore the Jacobian of the 
SCARA Manipulator is 


J = 


— aiSi — a 2 Si 2 —a 2 si 2 0 0 

aici + a 2 C \ 2 a 2 C \ 2 0 0 

0 0-10 

0 0 0 0 

0 0 0 0 

1 10-1 


(4.98) 
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4.8 THE ANALYTICAL JACOBIAN 


The Jacobian matrix derived above is sometimes called the Geometric Jacobian 
to distinguish it from the Analytical Jacobian , denoted J a {q ), considered in this 
section, which is based on a minimal representation for the orientation of the 
end-effector frame. Let 


d{q) 

a(q) 


(4.99) 


denote the end-effector pose, where d(q) is the usual vector from the origin of 
the base frame to the origin of the end-effector frame and a denotes a minimal 
representation for the orientation of the end-effector frame relative to the base 
frame. For example, let a = [cf> : 9, ip] T be a vector of Euler angles as defined in 
Chapter [2] Then we look for an expression of the form 


X = 


d 

a 


J a {q)q 


(4.100) 


to define the analytical Jacobian. 

It can be shown (Problem |4j[9]) that, if R = R z ,ipR y ^Rz,<p is the Euler angle 
transformation then 

R = S(u>)R (4.101) 

in which ui, defining the angular velocity is given by 


Tip Sgtp S^pO 

Sipsei> + c,p9 
ip + c s ip 


c^se 0 


’ </> ' 

SipSQ C'ljj 0 


0 

C0 0 1 _ 


. Tp . 


(4.102) 

= B{a) a (4.103) 


The components of u> are called the nutation , spin , and precession , respectively. 
Combining the above relationship with the previous definition of the Jacobian, 
i.e. 


V 


' d " 

id 


id 


J{q)q 


(4.104) 


yields 


J(q)q = 

V 

id 

= 

d 

B(a)a 


' I 

0 

■ 

d 


0 

B{ a 

) . 

a 


0 

B(a) 


J a {q)q 


(4.105) 

(4.106) 

(4.107) 


132 VELOCITY KINEMATICS - THE MANIPULATOR JACOBIAN 


Thus the analytical Jacobian, J a (q), 
Jacobian as 

Ja(q) = \ n 


may be computed from the geometric 


0 

B(a)- 1 


J{q) 


(4.108) 


provided det£?(a) / 0. 

In the next section we discuss the notion of Jacobian singularities, which are 
configurations where the Jacobian loses rank. Singularities of the matrix B(a) 
are called representational singularities. It can easily be shown (Problem |4] |20| ) 
that B(a) is invertible provided sg ^ 0. This means that the singularities of 
the analytical Jacobian include the singularities of the geometric Jacobian, J, 
as defined in the next section, together with the representational singularities. 


4.9 SINGULARITIES 


The 6 x n Jacobian J(q) defines a mapping 

£ = J(q)q (4-109) 

between the vector q of joint velocities and the vector £ = ( v,uj) t of end- 
effector velocities. This implies that the all possible end-effector velocities are 
linear combinations of the columns of the Jacobian matrix, 

£ = Jiq± + J 2 CI 2 • • • + J n qn 


For example, for the two-link planar arm, the Jacobian matrix given in Equation 
(4.861 has two columns. It is easy to see that the linear velocity of the end- 
effector must lie in the a;y-plane, since neither column has a nonzero entry for 
the third row. Since £ £ R 6 , it is necessary that J have six linearly independent 
columns for the end-effector to be able to achieve any arbitrary velocity (see 
Appendix [B]) . 

The rank of a matrix is the number of linearly independent columns (or 
rows) in the matrix. Thus, when rank J = 6, the end-effector can execute 


any arbitrary velocity. For a matrix J £ 


it is always the case that 


rank J < min(6, n). For example, for the two-link planar arm, we always have 
rank J < 2, while for an anthropomorphic arm with spherical wrist we always 
have rank J < 6. 

The rank of a matrix is not necessarily constant. Indeed, the rank of the 
manipulator Jacobian matrix will depend on the configuration q. Configurations 
for which the rank J(q) is less than its maximum value are called singularities 
or singular configurations. Identifying manipulator singularities is important 
for several reasons. 


1. Singularities represent configurations from which certain directions of motion 

may be unattainable. 

2. At singularities, bounded end-effector velocities may correspond to unbounded 

joint velocities. 
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3. At singularities, bounded end-effector forces and torques may correspond to 

unbounded joint torques. (We will see this in Chapter [9]). 

4. Singularities usually (but not always) correspond to points on the boundary 

of the manipulator workspace, that is, to points of maximum reach of the 
manipulator. 

5. Singularities correspond to points in the manipulator workspace that may 

be unreachable under small perturbations of the link parameters, such as 
length, offset, etc. 

6. Near singularities there will not exist a unique solution to the inverse kine- 

matics problem. In such cases there may be no solution or there may be 
infinitely many solutions. 

There are a number of methods that can be used to determine the singu- 
larities of the Jacobian. In this chapter, we will exploit the fact that a square 
matrix is singular when its determinant is equal to zero. In general, it is difficult 
to solve the nonlinear equation det J(q) = 0. Therefore, we now introduce the 
method of decoupling singularities, which is applicable whenever, for example, 
the manipulator is equipped with a spherical wrist. 

4.9.1 Decoupling of Singularities 

We saw in Chapter [3] that a set of forward kinematic equations can be derived 
for any manipulator by attaching a coordinate frame rigidly to each link in 
any manner that we choose, computing a set of homogeneous transformations 
relating the coordinate frames, and multiplying them together as needed. The 
DH convention is merely a systematic way to do this. Although the resulting 
equations are dependent on the coordinate frames chosen, the manipulator con- 
figurations themselves are geometric quantities, independent of the frames used 
to describe them. Recognizing this fact allows us to decouple the determination 
of singular configurations, for those manipulators with spherical wrists, into two 
simpler problems. The first is to determine so-called arm singularities, that 
is, singularities resulting from motion of the arm, which consists of the first 
three or more links, while the second is to determine the wrist singularities 
resulting from motion of the spherical wrist. 

For the sake of argument, suppose that n = 6, that is, the manipulator 
consists of a 3-DOF arm with a 3-DOF spherical wrist. In this case the Jacobian 
is a 6 x 6 matrix and a configuration q is singular if and only if 

det J(q) = 0 (4.110) 

If we now partition the Jacobian J into 3x3 blocks as 


J\ 1 

J\ 2 

J21 

J22 


J = [Jp | Jo] 


(4.111) 
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then, since the final three joints are always revolute 


Jo 


-3 X (06 - 0 3 ) Z4 X (o 6 - 04) Z 5 X (o 6 - 0 5) 
^3 2:4 25 


(4.112) 


Since the wrist axes intersect at a common point o, if we choose the coordi- 
nate frames so that 03 = 04 = 05 = oq = o, then Jq becomes 


Jo 


0 0 0 

Z3 Z4 z 5 


(4.113) 


In this case the Jacobian matrix has the block triangular form 


with determinant 


0 

J-22 


det J = det J n det J 2 2 


J = 


J\ 1 
J‘2\ 


(4.114) 


(4.115) 


where Jn and J 22 are each 3x3 matrices. Jn has i-tli column z,;_i x (o — Oj_i) 
if joint * is revolute, and Zj_i if joint i is prismatic, while 


J 22 — [~3 z 4 * 5 ] 


(4.116) 


Therefore the set of singular configurations of the manipulator is the union 
of the set of arm configurations satisfying det Jn = 0 and the set of wrist 
configurations satisfying det J 22 = 0. Note that this form of the Jacobian does 
not necessarily give the correct relation between the velocity of the end-effector 
and the joint velocities. It is intended only to simplify the determination of 
singularities. 


4.9.2 Wrist Singularities 


We can now see from Equation (4.1161 that a spherical wrist is in a singular 
configuration whenever the vectors Z3, Z4 and Z5 are linearly dependent. Refer- 
ring to Figure |4.3| we see that this happens when the joint axes Z 3 and z$ are 
collinear. In fact, when any two revolute joint axes are collinear a singularity 
results, since an equal and opposite rotation about the axes results in no net 
motion of the end-effector. This is the only singularity of the spherical wrist, 
and is unavoidable without imposing mechanical limits on the wrist design to 
restrict its motion in such a way that Z 3 and Z 5 are prevented from lining up. 


4.9.3 Arm Singularities 

To investigate arm singularities we need only to compute Jn, which is done 


using Equation (4.77 1 but with the wrist center o in place of o n . 
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Fig. 4.3 Spherical wrist singularity. 



Fig. 4.4 Elbow manipulator. 


Example 4.9 Elbow Manipulator Singularities 

Consider the three-link articulated manipulator with coordinate frames at- 
tached as shown in Figure 4-4 It left as an exercise (Problem\j\ 14\) to show 
that 


Ji i — 


— CI2S1C2 — CI3S1C23 — 02S2C1 — CI3S23C1 — «3ClS23 
Ct2CiC2 + 03C1C23 — G(2 SiS 2 — CI3S1S23 ~ <l3SlS23 

0 a 2 c 2 + a 3 c 23 a 3 c 23 


and that the determinant of J\\ is 

detJn = a2a 3 s 3 (a 2 C2 + a 3 c 2 3)- 


(4.117) 


(4.118) 


We see from Equation [4-118) that the elbow manipulator is in a singular 
configuration whenever 


S3 = 0, that is, (?3 = 0 or tt 


(4.119) 


and whenever 


02C2 + G3C23 


0 


( 4 . 120 ) 
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Fig. 4.5 Elbow singularities of the elbow manipulator. 


The situation of Equation (4-119) is shown in Figure 4-5 and arises when 
the elbow is fully extended or fully retracted as shown. The second situation 
of Equation (4-120) is shown in Figure 4-6 This configuration occurs when 


k z o 



9 1 



Fig. 4.6 Singularity of the elbow manipulator with no offsets. 

the wrist center intersects the axis of the base rotation, zq. As we saw in 
Chapter [$| there are infinitely many singular configurations and infinitely many 
solutions to the inverse position kinematics when the wrist center is along this 
axis. For an elbow manipulator with an offset, as shown in Figure the wrist 
center cannot intersect Zq, which corroborates our earlier statement that points 
reachable at singular configurations may not be reachable under arbitrarily small 
perturbations of the manipulator parameters, in this case an offset in either the 
elbow or the shoulder. 


o 
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Example 4.10 Spherical Manipulator 

Consider the spherical arm of Figure \ f.8\ This manipulator is in a singular 



Fig. 4.8 Singularity of spherical manipulator with no offsets. 

configuration when the wrist center intersects zq as shown since, as before, any 
rotation about the base leaves this point fixed, 
o 


Example 4.11 SCARA Manipulator 

We have already derived the complete Jacobian for the the SCARA manip- 
ulator. This Jacobian is simple enough to be used directly rather than deriving 
the modified Jacobian as we have done above. Referring to Figure \4 . 9\ we can 
see geometrically that the only singularity of the SCARA arm is when the elbow 
is fully extended or fully retracted. Indeed, since the portion of the Jacobian of 
the SCARA governing arm singularities is given as 


where 


Ji i — 


ol \ 03 0 
Oi 2 0:4 0 
0 0-1 


(4.121) 


ai 

= — OiSl — a2Si2 

(4.122) 

0 L 2 

= aiCi + CZ 2 C 12 


OLJ, 

= — 01 S 12 


Ct4 

= aiCi2 

(4.123) 


INVERSE VELOCITY AND ACCELERATION 139 


Zl Z 2 

I A 


02 = o° 



Fig. 4.9 SCARA manipulator singularity. 


we see that the rank of Jn will be less than three precisely whenever oqaq — 
0 : 20:3 = 0. It is easy to compute this quantity and show that it is equivalent to 
(Problem\4§16\) 

s 2 = 0, which implies 9 2 = 0,tt. (4.124) 




4.10 INVERSE VELOCITY AND ACCELERATION 


The Jacobian relationship 

£ = Jq (4.125) 

specifies the end-effector velocity that will result when the joints move with 
velocity q. The inverse velocity problem is the problem of finding the joint 
velocities q that produce the desired end-effector velocity. It is perhaps a bit 
surprising that the inverse velocity relationship is conceptually simpler than 
inverse position. When the Jacobian is square (i.e. , J £ R nxn ) and nonsingular, 
this problem can be solved by simply inverting the Jacobian matrix to give 

q = J _1 £ (4.126) 


For manipulators that do not have exactly six links, the Jacobian can not 
be inverted. In this case there will be a solution to Equation (4.125) if and 


only if £ lies in the range space of the Jacobian. This can be determined by the 
following simple rank test. A vector £ belongs to the range of J if and only if 


rankJ(< 7 ) = rank [J(q) | £] 


(4.127) 


In other words, Equation (4.1251 may be solved for q £ R n provided that the 
rank of the augmented matrix [</(<?) | £] is the same as the rank of the Jacobian 
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J(q). This is a standard result from linear algebra, and several algorithms exist, 
such as Gaussian elimination, for solving such systems of linear equations. 

For the case when n > 6 we can solve for q using the right pseudoinverse 
of J. To construct this psuedoinverse, we use the following result from linear 
algebra. 

Proposition: For J € R mxn , if m < n and rank J = m, then (JJ T ) 1 exists. 


In this case ( JJ T ) £ I mxra , and has rank m. Using this result, we can regroup 
terms to obtain 

(. JJ T )(JJ T )~ 1 = I 
J [J T (JJ T )~ 1 ] = I 
JJ+ = I 


Here, J + = J T (JJ T ) 1 is called a right pseudoinverse of J , since J J + = I. 
Note that, J + J £ R nxra , and that in general, J + J ^ I (recall that matrix 
multiplication is not commutative). 

It is now easy to demonstrate that a solution to Equation (|4.125 ) is given by 


q = J+£ + (I - J+J)b 


(4.128) 


in which b £ R n is an arbitrary vector. To see this, siimly multiply both sides 
of Equation (4.1281 by J: 


Jq = J[J+£+(I- J+J)b] 

= JJ+Z+ J(I- J+J)b 
= JJ+Z+ (J- JJ + J)b 
= £ + (J-J)b 
= € 


In general, for m < n, ( I—J + J ) ^ 0, and all vectors of the form ( I—J + J)b lie 
in the null space of J, i.e. , if q is a joint velocity vector such that q = ( I—J + J)b , 
then when the joints move with velocity q\ the end effector will remain fixed 
since Jq = 0. Thus, if q is a solution to Equation (4.1251, then so is q + q' with 
q' = (J — J + J)b 7 for any value of b. If the goal is to minimize the resulting joint 
velocities, we choose 6 = 0. To see this, apply the triangle inequality to obtain 


q II = || J + £+(/- J + J)b\\ 

< ii j + eii + n (/- j + j)mi 


It is a simple matter construct the right pseudoinverse of J using its singular 
value decomposition (see Appendix |B) , 


J+ = ve+u t 


MANIPULABILITY 141 


in which 


£+ = 


— 1 


r -i 


T 


We can apply a similar approach when the analytical Jacobian is used in 


place of the manipulator Jacobian. Recall from Equation (4.100) that the joint 


velocities and the end-effector velocities are related by the analytical Jacobian 


as 


X = J a (q)q (4.129) 

Thus the inverse velocity problem becomes one of solving the system of linear 
equations (4.129), which can be accomplished as above for the manipulator 
Jacobian. 

Differentiating Equation (4.129) yields the acceleration equations 


X = J a {q)q+ [ j f Ja{q) 


(4.130) 


Thus, given a vector X of end-effector accelerations, the instantaneous joint 
acceleration vector q is given as a solution of 

b = J a (q)q (4.131) 

where 

b = X-j t J a {q)q (4.132) 

For 6-DOF manipulators the inverse velocity and acceleration equations can 
therefore be written as 


q = J a {q ) % x 

and 

q = J a {q)~ 1 b 


provided det J a (q) 7^ 0. 


(4.133) 


(4.134) 


4.11 MANIPULABILITY 

For a specific value of q , the Jacobian relationship defines the linear system 
given by £ = Jq. We can think of J a scaling the input, q, to produce the 
output, £. It is often useful to characterize quantitatively the effects of this 
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scaling. Often, in systems with a single input and a single output, this kind of 
characterization is given in terms of the so called impulse response of a system, 
which essentially characterizes how the system responds to a unit input. In 
this multidimensional case, the analogous concept is to characterize the output 
in terms of an input that has unit norm. Consider the set of all robot joint 
velocities q such that 

MW = Ml + ql + ■■■q 2 m ) 1/2 < i (4.135) 


If we use the minimum norm solution q = J + £, we obtain 


• t • 

q q 


= (j + o T J + a 


= e\jJ T )-^< i 


(4.136) 


The derivation of this is left as an exercise (Problem |4]|26|). This final inequality 
gives us a quantitative characterization of the scaling that is effected by the 
Jacobian. In particular, if the manipulator Jacobian is full rank, i.e. , rank J = 
to, then Equation ( 4.136[ ) defines an m-dimensional ellipsoid that is known as 
the manipulability ellipsoid. If the input (i.e., joint velocity) vector has unit 
norm, then the output (i.e., end-effector velocity) will lie within the ellipsoid 
given by Equation (4.136 1. We can more easily see that Equation ( 4.136| ) defines 
an ellipsoid by replacing the Jacobian by its SVD J = UHV T (see Appendix 
E to obtain 

C T (JJ T )-^ T = (U T 0 T X~ 2 (U T 0 (4.137) 


in which 


y - 2 — 


—2 


The derivation of Equation (4.137) is left as an exercis e (Problem 4p7). If we 
make the substitution w = U T £, then Equation (4.137) can be written as 


T x ~\ — 2 

w w 


= £ 


<7- V < 1 


(4.138) 


and it is clear that this is the equation for an axis-aligned ellipse in a new 
coordinate system that is obtained by rotation according to the orthogonal 
matrix U. In the original coordinate system, the axes of the ellipsoid are given 
by the vectors oyiq. The volume of the ellipsoid is given by 


volume = KaiCT 2 ■ ■ ■ <J m 


in which K is a constant that depends only on the dimension, to, of the ellipsoid. 
The manipulability measure, as defined by Yoshikawa [78] , is given by 


JL — a 102 • • • cr m 


(4.139) 
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Note that the constant K is not included in the definition of manipulability, 
since it is fixed once the task has been defined (i.e., once the dimension of the 
task space has been fixed). 

Now, consider the special case that the robot is not redundant, i.e., J £ 
E mxm . Recall that the determinant of a product is equal to the product of the 
determinants, and that a matrix and its transpose have the same determinant. 
Thus, we have 

det JJ T = det J det J T 
= det J det J 
= (A 1 A 2 • • • A m )(AiA2 • • • A m ) 

= A?Al - - - A^ (4.140) 

in which Ai > A 2 • • ■ < A m are the eigenvalues of J. This leads to 

/x = \J det JJ T = IA 1 A 2 • • • A m | = |det J | (4.141) 


The manipulability, p, has the following properties. 


• In general, /i = 0 holds if and only if rank(J) < m, (i.e., when J is not 
full rank). 

• Suppose that there is some error in the measured velocity, A£. We can 
bound the corresponding error in the computed joint velocity, A q, by 


/ \-l ^ Ill'll ^ / wl 
( l} -||AC||- ( m) 


(4.142) 


Example 4.12 Two-link Planar Arm. 

We can use manipulability to determine the optimal configurations in which 
to perform certain tasks. In some cases it is desirable to perform a task in the 
configuration for which the end effector has the maximum dexterity. We can 
use manipulability as a measure of dexterity. Consider the two-link planar arm 
and the task of positioning in the plane. For the two link arm, the Jacobian is 
given by 


—aiS\ — CI2S12 — U2S12 
aqci + a 2 ci2 CI2C12 

and the manipulability is given by 

p = | det J | = aia 2 |s 2 | 



(4.143) 


Thus, for the two-link arm, the maximum manipulability is obtained for 62 = 
±tt/2. 

Manipulability can also be used to aid in the design of manipulators. For 
example, suppose that we wish to design a two-link planar arm whose total link 
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length , ai + 02 , is fixed. What values should be chosen for a\ and 02 ? If 
we design the robot to maximize the maximum manipidability, the we need to 
maximize p = asio^^l- We have already seen that the maximum is obtained 
when 02 = ±7r/2, so we need only find ai and 02 to maximize the product a\a 2 - 
This is achieved when ai = 02 ■ Thus, to maximize manipidability, the link 
lengths should be chosen to be equal, 
o 


4.12 CHAPTER SUMMARY 


A moving coordinate frame has both a linear and an angular velocity. Linear 
velocity is associated to a moving point, while angular velocity is associated 
to a rotating frame. Thus, the linear velocity of a moving frame is merely 
the velocity of its origin. The angular velocity for a moving frame is related 
to the time derivative of the rotation matrix that describes the instantaneous 
orientation of the frame. In particular, if R(t) £ 5*0(3), then 

R(t) = S(w(t))R(t) (4.144) 


and the vector ui(t) is the instantaneous angular velocity of the frame. The 
operator S gives a skew symmetrix matrix 


s'H 


0 ~(jJ z LVy 

uj z 0 -Wj 

U)y L0 X 0 


(4.145) 


The manipulator Jacobian relates the vector of joint velocities to the body 
velocity £ = ( v,u>) T of the end effector, 


f=Jq 


(4.146) 


This relationship can be written as two equations, one for linear velocity and 
one for angular velocity, 


v = J v q (4.147) 

w = J^q (4.148) 


The i-th column of the Jacobian matrix corresponds to the i-th joint of the 
robot manipulator, and takes one of two forms depending on whether the i-th 
joint is prismatic or revolute 


Zi-l X (o n - Oj_ 1) 
Zi-l 


if joint i is revolute 


Zi-l 

0 


Ji={ 


if joint i is prismatic 


(4.149) 
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For a given parameterization of orientation, e.g. Euler angles, the analytical 
Jacobian relates joint velocities to the time derivative of the pose parameters 


' d{q) ' 

x — 

' d ' 

_ at(q) _ 

A — 

a 


J a {q)q 


iin which d{q) is the usual vector from the origin of the base frame to the origin 
of the end-effector frame and a denotes a parameterization of rotation matrix 
that specifies the orientation of the end-effector frame relative to the base frame. 
For the Euler angle parameterization, the analytical Jacobian is given by 


Ja{q) 


I 0 
0 B(a)- 1 


J{q) 


(4.150) 


in which 


B(a) 


C,pSg —Sij, 0 

9 T-ip 0 

eg 0 1 


A configuration at which the Jacobian loses rank (i.e., a configuration q such 
that rank J < max, rank J(q)) is called a singularity. For a manipulator with 
a spherical wrist, the set of singular configurations includes singular ites of the 
wrist (which are merely the singularities in the Euler angle parameterization) 
and singularites in the arm. The latter can be found by solving 


clet Jn = 0 

with Jn the upper left 3x3 block of the manipulator Jacobian. 

For nonsingular configurations, the Jacobian relationship can be used to find 
the joint velocities q necessary to achieve a desired end-effector velocity £ The 
minimum norm solution is given by 

q = J + Z 

in which J + = J T (J J T )~ 1 is the right pseudoinverse of J. 

Manipulability is defined by pi = ■ ■ • a m in which cr; are the singular 

values for the manipulator Jacobian. The manipulatibility can be used to char- 
acterize the range of possible end-effector velocities for a given configuration 

q ■ 
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Problems 


4-1 

4-2 

4-3 

4-4 

4-5 

4-6 


Verify Equation (4.6 1 by direct calculation. 


Verify Equation (4.7 1 by direct calculation. 


Verify Equation (4.8 1 by direct calculation. 


Verify Equation (4.161 by direct calculation. 

Show that S(k) 3 = — S(k). Use this and Problem 
tion ( |4.17| ). 


25 


to 


Given any square matrix A, the exponential of A is a matrix 


= I + A- 


1 


A 2 




verify Equa- 
defined as 


Given S £ so(3) show that e s £ SO( 3). 

[Hint: Verify the facts that e A e B = e A+B provided that A and B commute, 
that is, AB = BA, and also that det(e A ) = e Tr ^ A \] 


4-7 Show that R k g = e s ^ e . 


[Hint: 

Problems 25 
ential equation 


Use the series expansion for the matrix exponential together with 
and|5j Alternatively use the fact that R k g satisfies the differ- 


f = S ^ K 


4-8 Use Problem [7] to show the converse of Problem [6] that is, if R £ SO( 3) 
then there exists S £ so(3) such that R = e s . 

4-9 Given the Euler angle transformation 

R Rz,il)Ry,QRz,<t> 

show that 4j:R = S(lo)R where 

u> = {c^,se<p — s^6}i + {s^sgifi + c^9}j + {[s* + cg(j)}k. 

The components of i,j,k, respectively, are called the nutation, spin, and 
precession. 


4-10 Repeat Problem [9] for the Roll-Pitch-Yaw transformation. In other words, 
find an explicit expression for ui such that = S(lu)R, where R is given 
by Equation ( |2.39[ ). 
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4-11 


4-12 


4-13 

4-14 

4-15 

4-16 

4-17 

4-18 

4-19 

4-20 

4-21 


Two frames ooXo?/o~o and o\Xiy\Zi 

are related by the homogeneous trans- 

formation 

' 0 

-1 

0 

1 ' 


H = 

1 

0 

0 

-1 


0 

0 

1 

0 



0 

0 

0 

1 


A particle has velocity V\(t) = 

(3,1, Of 

relative to frame 0 \XiyiZ\. What 


is the velocity of the particle in frame oo^oj/o^o? 

Three frames oqXqUqZq and o\X\y\Z\, and 02 X 2 IJ 2 Z 2 are given below. If the 
angular velocities and are given as 



' 1 ' 


' 2 ' 

»? = 

1 

i w 2 ~ 

0 


1 

O 

1 


1 


what is the angular velocity cf at the instant when 


R 


0 

1 


10 0 

0 0-1 

0 1 0 


). For the three-link planar manipulator of Example 4.6 compute the 
vector O c and derive the manipulator Jacobian matrix. 

Compute the Jacobian Jn for the 3-link elbow manipulator of Example |4. 9 1 
and show that it agrees with Equation (4.117 1. Show that the determinant 
of this matrix agrees with Equation (4.1181. 

Compute the Jacobian Jn for the three-link spherical manipulator of Ex- 


ample 4.10 


Show from Equation (4.1221 that the singularities of the SCARA manipu- 
lator are given by Equation (4.1241. 

Find the 6x3 Jacobian for the three links of the cylindrical manipulator 
of Figure [3~~7] Show that there are no singular configurations for this arm. 
Thus the only singularities for the cylindrical manipulator must come from 
the wrist. 


Repeat Problem 17 for the cartesian manipulator of Figure 3.28 


Complete the derivation of the Jacobian for the Stanford manipulator from 
Example |4.7[ 


Show that B{a) given by Equation (4.103) is invertible provided sg fy 0. 


Verify Equation (4.7 1 by direct calculation. 
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4-22 Prove the assertion given in Equation (4.8 1 that R(a x b) = Ra x Rb 1 for 
R e 50(3). 

4-23 Suppose that a = (1, — 1,2) T and that R = R Xi 90 . Show by direct calcula- 
tion that 

RS{a)R T = S(Ra). 

4-24 Given R° = R x ,oRy,<l> , compute Evaluate at 6 = ^ 


4-25 Use Equation (2.46) to show that 


0 = I + S(k) sin(0) + 5 2 (fc) vers(0). 


4-26 Verify Equation (4.1361. 


4-27 Verify Equation (4.1371. 


5 


PATH AND 
TRAJECTORY 
PLANNING 


In previous chapters, we have studied the geometry of robot arms, developing 
solutions for both the forward and inverse kinematics problems. The solutions 
to these problems depend only on the intrinsic geometry of the robot, and they 
do not reflect any constraints imposed by the workspace in which the robot 
operates. In particular, they do not take into account the possiblity of collision 
between the robot and objects in the workspace. In this chapter, we address the 
problem of planning collision free paths for the robot. We will assume that the 
initial and final configurations of the robot are specified, and that the problem 
is to find a collision free path for the robot that connects them. 

The description of this problem is deceptively simple, yet the path planning 
problem is among the most difficult problems in computer science. The compu- 
tational complexity of the best known complet^jpath planning algorithm grows 
exponentially with the number of internal degrees of freedom of the robot. For 
this reason, for robot systems with more than a few degrees of freedom, com- 
plete algorithms are not used in practice. 

In this chapter we treat the path planning problem as a search problem. The 
algorithms we describe are not guaranteed to find a solution to all problems, 
but they are quite effective in a wide range of practical applications. Further- 
more, these algorithms are fairly easy to implement, and require only moderate 
computation time for most problems. 


1 An algorithm is said to be complete if it finds a solution whenever one exists. 
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Path planning provides a geometric description of robot motion, but it does 
not specify any dynamic aspects of the motion. For example, what should be the 
joint velocities and accelerations while traversing the path? These questions are 
addressed by a trajectory planner. The trajectory planner computes a function 
q d (t) that completely specifies the motion of the robot as it traverses the path. 


We begin in Section [5.1| by introducing the notion of configuration space , and 
describing how obstacles in the workspace can be mapped into the configuration 
space. We then introduce path planning methods that use artificial potential 
fields in Sections [5~2] and [T3| The corresponding algorithms use gradient descent 


search to find a collision-free path to the goal, and, as with all gradient descent 
methods, these algorithms can become trapped in local minima in the potential 
field. Therefore, in Section |5.4| we describe how random motions can be used 
to escape local minima. In Section [53] we describe another randomized method 
known as the Probabilistic Roadmap (PRM) method. Finally, since each of 
these methods generates a sequence of configurations, we describe in Section 


5.6 how polynomial splines can be used to generate smooth trajectories from a 


sequence of configurations. 


5.1 THE CONFIGURATION SPACE 

In Chapter [3] we learned that the forward kinematic map can be used to deter- 
mine the position and orientation of the end effector frame given the vector of 
joint variables. Furthermore, the matrices can be used to infer the position 
and orientation of the reference frame for any link of the robot. Since each link 
of the robot is assumed to be a rigid body, the A t matrices can therefore be 
used to infer the position of any point on the robot, given the values of the 
joint variables. In the path planning literature, a complete specification of the 
location of every point on the robot is referred to as a configuration , and the set 
of all possible configurations is referred to as the configuration space. For our 
purposes, the vector of joint variables, q , provides a convenient representation 
of a configuration. We will denote the configuration space by Q. 

For a one link revolute arm, the configuration space is merely the set of 
orientations of the link, and thus Q = S 1 , where S 1 represents the unit circle. 
We could also say Q = SO( 2). In fact, the choice of S' 1 or 50(2) is not 
particularly important, since these two are equivalent representations. In either 
case, we can parameterize Q by a single parameter, the joint angle 9\. For the 
two- link planar arm, we have Q = S 1 x S 1 = T 2 , in which T 2 represents the 
torus, and we can represent a configuration by q = ( 81 , 62 )• For a Cartesian 
arm, we have Q = 5ft 3 , and we can represent a configuration by q = (c?i, d 2 ,d^) = 
( x,y,z ). 

Although we have chosen to represent a configuration by a vector of joint 
variables, the notion of a configuration is more general than this. For example, 
as we saw in Chapter [2] for any rigid two-dimensional object, we can specify the 
location of every point on the object by rigidly attaching a coordinate frame 
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to the object, and then specifying the position and orientation of this frame. 
Thus, for a rigid object moving in the plane we can represent a configuration 
by the triple q = (x,y,6), and the configuration space can be represented by 
Q — 5ft 2 x SO( 2). Again, this is merely one possible representation of the 
configuration space, but it is a convenient one given the representations of 
position and orientation that we have learned in preceeding chapters. 

A collision occurs when the robot contacts an obstacle in the workspace. 
To describe collisions, we introduce some additional notation. We will denote 
the robot by A , and by A{q) the subset of the workspace that is occupied by 
the robot at configuration q. We denote by Oi the obstacles in the workspace, 
and by W the workspace (i.e., the Cartesian space in which the robot moves). 
To plan a collision free path, we must ensure that the robot never reaches a 
configuration q that causes it to make contact with an obstacle in the workspace. 
The set of configurations for which the robot collides with an obstacle is referred 
to as the configuration space obstacle, and it is defined by 

QO = {q € Q I A{q) flO/0} 

Here, we define O = UCV The set of collision-free configurations, referred to 
as the free configuration space, is then simply 

Qfree = Q \ QO 


Example 5.1 A Rigid Body that Translates in the Plane. 




Fig. 5.1 (a) a rigid body, A, in a workspace containing a single rectangular obstacle, 

O, (b) illustration of the algorithm to construct QO , with the boundary of QO shown 
as the dashed line 
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Consider a simple gantry robot with two prismatic joints and forward kine- 
matics given by x = d\,y = d 2 . For this case, the robot's configuration space 
is Q = 5i 2 , so it is particularly easy to visualize both the configuration space 
and the configuration space obstacle region. If there is only one obstacle in the 
workspace and both the robot end-effector and the obstacle are convex polygons, 
it is a simple matter to compute the configuration space obstacle region, QO (we 
assume here that the arm itself is positioned above the workspace, so that the 
only possible collisions are between the end-effector and obstacles the obstacle). 

Let V.^ denote the vector that is normal to the i th edge of the robot and Vf 
denote the vector that is normal to the i th edge of the obstacle. Define ai to be 
the vector from the origin of the robot’s coordinate frame to the i th vertex of 
the robot and bj to be the vector from the origin of the world coordinate frame 


to the j th vertex of the obstacle. An example is shown in Figure 5.1(a). The 
vertices of QO can be determined as follows. 

• For each pair V® and V^_ 1 , if VA points between —V® and then 

add to QO the vertices bj — ai and bj — a,+i. 

• For each pair VA and if V® points between —VA and —Vf) 1 then 

add to QO the vertices bj — ai and bj + 1 — a*. 


This is illustrated in Figure Note that this algorithm essentially places 

the robot at all positions where vertex-vertex contact between robot and obsta- 
cle are possible. The origin of the robot’s local coordinate frame at each such 
configuration defines a vertex of QO. The polygon defined by these vertices is 

QO. 

If there are multiple convex obstacles Oi, then the configuration space obsta- 
cle region is merely the union of the obstacle regions QOi, for the individual 
obstacles. For a nonconvex obstacle, the configuration space obstacle region can 
be computed by first decomposing the nonconvex obstacle into convex pieces, 
Oi, computing the C-space obstacle region, QOi for each piece, and finally, 
computing the union of the QOi . 
o 


Example 5.2 A Two Link Planar Arm. 

For robots with revolute joints, computation of QO is more difficult. Con- 
sider a two-link planar arm in a workspace containing a single obstacle as shown 


in Figure \5Jy{ a). The configuration space obstacle region is illustrated in 5.2(b). 
The horizontal axis in \5.0( b) corresponds to and the vertical axis to 0 2 . For 
values of 6 1 very near 7r/2, the first link of the arm collides with the obstacle. 
Further, when the first link is near the obstacle ( 6 1 near n/Z), for some values 
of d 2 the second link of the arm collides with the obstacle. The region QO shown 
5.2^b) was computed using a discrete grid on the configuration space. For 


m 


each cell in the grid, a collision test was performed, and the cell was shaded 
when a collision occured. Thus, this is only an approximate representation of 


QO. 
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(a) 


(b) 


Fig. 5.2 (a) A two-link planar arm and a single polygonal obstacle, (b) The corre- 
sponding configuration space obstacle region 


Computing QO for the two-dimensional case of Q = 3? 2 and polygonal ob- 
stacles is straightforward, but, as can be seen from the two-link planar arm 
example, computing QO becomes difficult for even moderately complex config- 
uration spaces. In the general case (e.g., articulated arms or rigid bodies that 
can both translate and rotate), the problem of computing a representation of 
the configuration space obstacle region is intractable. One of the reasons for 
this complexity is that the size of the representation of the configuration space 
tends to grow exponentially with the number of degrees of freedom. This is 
easy to understand intuitively by considering the number of n-dimensional unit 
cubes needed to fill a space of size k. For the one dimensional case, k unit inter- 
vals will cover the space. For the two-dimensional case, k 2 squares are required. 
For the three-dimensional case, k 3 cubes are required, and so on. Therefore, in 
this chapter we will develop methods that avoid the construction of an explicit 
representation of <2f ree - 

The path planning problem is to find a path from an initial configuration 
( 7 init to a final configuration < 7 fina i, such that the robot does not collide with 
any obstacle as it traverses the path. More formally, A collision-free path from 
g init to Qf} na i is a continuous map, r : [0, 1] — > <2f ree , with r(0) = g init and 
r(l) = 9 flna j. We will develop path planning methods that compute a sequence 


of discrete configurations (set points) in the configuration space. In Section 5.6 


we will show how smooth trajectories can be generated from such a sequence. 
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5.2 PATH PLANNING USING CONFIGURATION SPACE POTENTIAL 
FIELDS 

As mentioned above, it is not feasible to build an explicit representation of Qf ree . 
An alternative is to develop a search algorithm that incrementally explores 
Qfree while searching for a path. Such a search algorithm requires a strategy 
for exploring Qf ree , and one of the most popular is to use an artificial potential 
field to guide the search. In this section, we will introduce artificial potential 
field methods. Here we describe how the potential field can be constructed 
directly on the configuration space of the robot. However, as will become clear, 
computing the gradient of such a field is not feasible in general, so in Section [573] 
we will develop an alternative, in which the potential field is first constructed 
on the workspace, and then its effects are mapped to the configuration space. 

The basic idea behind the potential field approaches is as follows. The robot 
is treated as a point particle in the configuration space, under the influence of 
an artificial potential field U. The field U is constructed so that the robot is 
attracted to the final configuration, <7fj na i , while being repelled from the bound- 
aries of QO. If U is constructed appropriately, there will be a single global 
minimum of U at <7 flnal , and no local minima. Unfortunately, as we will discuss 
below, it is often difficult to construct such a field. 

In general, the field U is an additive field consisting of one component that 
attracts the robot to (?g na i and a second component that repels the robot from 
the boundary of QO, 


U(q) = U att (q) + U rep {q) (5.1) 

Given this formulation, path planning can be treated as an optimization 
problem, i.e., find the global minimum in U, starting from initial configuration 
f/ init . One of the easiest algorithms to solve this problem is gradient descent. 
In this case, the negative gradient of U can be considered as a force acting on 
the robot (in configuration space), 

F(g) = -VU(q) = —'VUg.tt(q) - VC/ rep (g) (5.2) 

In the remainder of this section, we will describe typical choices for the 
attractive and repulsive potential fields, and a gradient descent algorithm that 
can be used to plan paths in this field. 

5.2.1 The Attractive Field 

There are several criteria that the potential field U a tt should satisfy. First, 
U a tt should be monotonically increasing with distance from <?fi na i- The simplest 
choice for such a field is a field that grows linearly with the distance from <jr final , 
a so-called conic well potential. However, the gradient of such a field has unit 
magnitude everywhere but the origin, where it is zero. This can lead to stability 
problems, since there is a discontinuity in the attractive force at the origin. We 
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prefer a field that is continuously differentiable, such that the attractive force 
decreases as the robot approaches g final . The simplest such field is a field that 
grows quadratically with the distance to q finaI . Let p/(q) be the Euclidean 
distance between q and < 7 flnal , i.e., Pf(q) = ||q — < 7 final ||. Then we can define the 
quadratic field by 


U,tt(q) = (5.3) 

in which £ is a parameter used to scale the effects of the attractive potential. 
This field is sometimes referred to as a parabolic well. For q = (q 1 , ■ ■ ■ q n ) T , the 
gradient of U a tt is given by 


VEMg) = V^C P 2 f(q) 

= ^ 2 ^^ _ Qfinal 1 1 " 

= 2 CV E(^9final) 2 
= C ( < ? 1 ~ 9 final > " • lQ n ~ 9 final) T 

C(Q 9 final ) 


Here, (5.4 1 follows since 


~ ^final) 2 = 2(g J - qL ai) 


(5.4) 


So, for the parabolic well, the attractve force, F att (q) = — Vf7 at t(g) is a vector 
directed toward <jg na i with magnitude linearly related to the distance from q to 

9 final ■ 

Note that while F at t(q) converges linearly to zero as q approaches g flna j (which 
is a desirable property), it grows without bound as q moves away from g flnal . If 
< 7 init is very far from q final , this may produce an attractive force that is too large. 
For this reason, we may choose to combine the quadratic and conic potentials 
so that the conic potential attracts the robot when it is very distant from < 7 final 
and the quadratic potential attracts the robot when it is near g final . Of course 
it is necessary that the gradient be defined at the boundary between the conic 
and quadratic portions. Such a field can be defined by 


U at t{q) 


d(Pf(q ) - d 2 


Pf(q) < d 
Pf(q) > d 


(5.5) 
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and in this case we have 


F att (q) = -VC/ att (g) = 


C(«Z *Zfinal) 
^Zfinal) 

Pf(q) 


pf(q) < d 


p f (q) > d 


(5.6) 


The gradient is well defined at the boundary of the two fields since at the 
boundary d = Pf(q), and the gradient of the quadratic potential is equal to the 
gradient of the conic potential, F att (q) — — C (<Z — 9finai)- 


5.2.2 The Repulsive field 

There are several criteria that the repulsive field should satisfy. It should repel 
the robot from obstacles, never allowing the robot to collide with an obstacle, 
and, when the robot is far away from an obstacle, that obstacle should exert 
little or no influence on the motion of the robot. One way to achieve this is to 
define a potential that goes to infinity at obstacle boundaries, and drops to zero 
at a certain distance from the obstacle. If we define po to be the distance of 
influence of an obstace (i.e., an obstacle will not repel the robot if the distance 
from the robot to the obstacle is greater that po), one potential that meets 
these criteria is given by 


U le p {q) — 


1 

Po 


2 V \p(q) 


0 


p(q) < Po 


p(q) > po 


(5.7) 


in which p(q) is the shortest distance from q to a configuration space obstacle 
boundary, and 77 is a scalar gain coefficient that determines the influence of 
the repulsive field. If QO consists of a single convex region, the corresponding 
repulsive force is given by the negative gradient of the repulsive field, 


Frep{q) — 


(p(<z) 


1 

Po 


p 2 (q) 


0 


Vp(g) : p(q) < Po 


p(q) > Po 


(5.8) 


When QO is convex, the gradient of the distance to the nearest obstacle is given 
by 

V P(<7) = (5-9) 

in which b is the point in the boundary of QO that is nearest to q. 
derivation of (15.81) and 15.91) are left as exercises ??. 


The 


If QO is not convex, then p won’t necessarily be differentiable everywhere, 
which implies discontinuity in the force vector. Figure [5T3|illustrates such a case. 
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Fig. 5.3 

tinuous 


Situation in which the gradient of the repuslive potential of (5.7 1 is not con- 


Here QO contains two rectangular obstacles. For all configurations to the left of 
the dashed line, the force vector points to the right, while for all configurations 
to the right of the dashed line the force vector points to the left. Thus, when 
the configuration of the robot crosses the dashed line, a discontinuity in force 
occurs. There are various ways to deal with this problem. The simplest of 
these is merely to ensure that the regions of influence of distinct obstacles do 
not overlap. 


5.2.3 Gradient Descent Planning 


Gradient descent is a well known approach for solving optimization problems. 
The idea is simple. Starting at the initial configuration, take a small step in 
the direction of the negative gradient (i.e., in the direction that decreases the 
potential as quickly as possible) . This gives a new configuration, and the process 
is repeated until the final configuration is reached. More formally, we can define 
a gradient descent algorithm as follows. 


1 . 

2 . 


3. 


9° <- QiniV * 0 

IF <f ± 9final 

q i+ 1 <- q i + a 1 


F(q l ) 

ITODII 


i <— i + 1 

ELSE return < q°, q 1 ■ ■ ■ q i > 

GO TO 2 


In this algorithm, the notation q l is used to denote the value of q at the i th 
iteration (not the i th componenent of the vector q) and the final path consists 
of the sequence of iterates < g°, q 1 ■ ■ ■ q 1 >. The value of the scalar a 1 deter- 
mines the step size at the i th iteration; it is multiplied by the unit vector in the 
direction of the resultant force. It is important that a 1 be small enough that 
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• Q final 



Qinit 

Fig. 5.4 This figure illustrates the progress of a gradient descent algorithm from g init 
to a local minimum in the field U 


the robot is not allowed to “jump into” obstacles, while being large enough that 
the algorithm doesn’t require excessive computation time. In motion planning 
problems, the choice for a 1 is often made on an ad hoc or empirical basis, per- 
haps based on the distance to the nearest obstacle or to the goal. A number of 
systematic methods for choosing a 1 can be found in the optimization literature 
[7\. The constants ( and rj used to define U at t and U rep essentially arbitrate 
between attractive and repulsive forces. Finally, it is unlikely that we will ever 
exactly satisfy the condition q l = g final . For this reason, this condition is often 
replaced with the more forgiving condition 1 1 q l — g final 1 1 < e, in which e is chosen 
to be sufficiently small, based on the task requirements. 

The problem that plagues all gradient descent algorithms is the possible 
existence of local minima in the potential field. For appropriate choice of cd, 
it can be shown that the gradient descent algorithm is guaranteed to converge 
to a minimum in the field, but there is no guarantee that this minimum will be 
the global minimum. In our case, this implies that there is no guarantee that 
this method will find a path to 9fj na i- An example of this situation is shown in 


Figure 5.4 We will discuss ways to deal this below in Section 5.4 


One of the main difficulties with this planning approach lies in the evaluation 
of p and Vp. In the general case, in which both rotational and translational 
degrees of freedom are allowed, this becomes even more difficult. We address 
this general case in the next section. 


5.3 PLANNING USING WORKSPACE POTENTIAL FIELDS 

As described above, in the general case, it is extremely difficult to compute an 
explicit representation of QO, and thus it can be extremely difficult to compute 
p and Vp. In fact, in general for a curved surface there does not exist a closed 
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form expression for the distance from a point to the surface. Thus, even if 
we had access to an explicit representation of QO , it would still be difficult 
to compute p and Vp in (5.8 1 . In order to deal with these difficulties, in this 


section we will modify the potential field approach of Section |5.2| so that the 
potential function is defined on the workspace, W, instead of the configuration 
space, Q. Since W is a subset of a low dimensional space (either SR 2 or R 3 ), it 
will be much easier to implement and evaluate potential functions over W than 
over Q. 

We begin by describing a method to define an appropriate potential field 
on the workspace. This field should have the properties that the potential is 
at a minimum when the robot is in its goal configuration, and the potential 
should grow without bound as the robot approaches an obstacle. As above, we 
will define a global potential field that is the sum of attractive and repulsive 
fields. Once we have constructed the workspace potential, we will develop the 
tools to map its gradient to changes in the joint variable values (i.e., we will 
map workspace forces to changes in configuration). Finally, we will present a 
gradient descent algorithm similar to the one presented above, but which can 
be applied to robots with more complicated kinematics. 


5.3.1 Defining Workspace Potential Fields 

As before, our goal in defining potential functions is to construct a field that 
repels the robot from obstacles, with a global minimum that corresponds to 
g final . In the configuration space, this task was conceptually simple because the 
robot was represented by a single point, which we treated as a point mass under 
the influence of a potential field. In the workspace, things are not so simple; 
the robot is an articulated arm with finite volume. Evaluating the effect of 
a potential field over the arm would involve computing an integral over the 
volume of the arm, and this can be quite complex (both mathematically and 
computationally). An alternative approach is to select a subset of points on 
the robot, called control points, and to define a workspace potential for each 
of these points. The global potential is obtained by summing the effects of the 
individual potential functions. Evaluating the effect a particular potential field 
on a single point is no different than the evaluations required in Section |5.2[ 
but the required distance and gradient calculations are much simpler. 

Let Aatt = {ai, 02 • • • a n } be a set of control points used to define the at- 
tractive potential fields. For an n-link arm, we could use the centers of mass 
for the n links, or the origins for the DH frames (excluding the fixed frame 0). 
We denote by a,i(q) the position of the i th control point when the robot is at 
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configuration q. For each cq 6 -A a tt we can define an attractive potential by 

{ RilMe) - ai(9finai)ll 2 : IM<?) - a*(gfi nal )ll < d 

I 

^Cilki(g) - ai(g fi nai)ll - 9 Cid 2 : IM<?) - a l (g final )|| > d 

(5-10) 

For the single point a*, this function is analogous the attractive potential defined 
in Section |5.2| it combines the conic and quadratic potentials, and reaches 
its global minimum when the control point reaches its goal position in the 
workspace. If «4 a tt contains a sufficient number of independent control points 
(the origins of the DH frames, e.g.), then when all control points reach their 
global minimum potential value, the configuration of the arm will be q final . 

With this potential function, the workspace force for attractive control point 
a,i is defined by 




— Vf7 at ti(g) 


~ M^nal)) 
dCi(ai(q) - Qi(gfinal)) 

\\a,(q)-adq fIlwl )W 


(5.11) 

aM) -a*(gfi nal )|| < d 

(5.12) 

a-i(q) - a»(gfinai)ll > d 


For the workspace repulsive potential fields, we will select a set of fixed con- 
trol points on the robot -4 rep = {ai, ■ ■ ■ , a m }, and define the repulsive potential 
for Oj as 


Ure Pj (q) = 


1 

2^ 


\P(aj{q)) 



P(aj(q)) < Po 


(5.13) 


l 0 : p{aj{q )) > p 0 

in which p{aj{q)) is the shortest distance between the control point aj and 
any workspace obstacle, and po is the workspace distance of influence in the 
worksoace for obstacles. The negative gradient of each t/ rePj - corresponds to a 
workspace repulsive force, 


W9) = | _ h) rnm Vl,{a ’ {q)) ■ ' >w,)) - 

{ 0 = P{aj(q))>p 0 

( 5 - 14 ) 

in which the notation \7p(aj(q)) indicates the gradient Vp(x) evaluated at x = 
o,j(q). If b is the point on the workspace obstacle boundary that is closest to 
the repulsive control point aj, then p{aj{q)) = \\aj(q) — i>| | , and its gradient is 

x=a j {q) \\aj(q)-b\\ 


X7p(x) 


(5.15) 
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Fig. 5.5 The repulsive forces exerted on the robot vertices ai and 02 may not be suffi- 
cient to prevent a collision between edge Ei and the obstacle 


i.e., the unit vector directed from b toward a,j(q). 

It is important to note that this selection of repulsive control points does not 
guarantee that the robot cannot collide with an obstacle. Figure [575] shows an 
example where this is the case. In this figure, the repulsive control points ai 
and a 2 are very far from the obstacle O , and therefore the repulsive influence 
may not be great enough to prevent the robot edge E\ from colliding with the 
obstacle. To cope with this problem, we can use a set of floating repulsive 
control points, dfi oat typically one per link of the robot arm. The floating 
control points are defined as points on the boundary of a link that are closest 
to any workspace obstacle. Obviously the choice of the a,fi oa t,i depends on the 
configuration q. For the example shown in Figure [575} afi oa t would be located at 
the center of edge E±, thus repelling the robot from the obstacle. The repulsive 
force acting on a fi oat is defined in the same way as for the other control points, 
using (5.141. 


5.3.2 Mapping workspace forces to joint forces and torques 

Above we have constrructed potential fields in the robot’s workspace, and these 
fields induce artificial forces on the individual control points on the robot. In 
this section, we describe how these forces can be used to drive a gradient descent 
algorithm on the configuration space. 

Suppose a force, T were applied to a point on the robot arm. Such a force 
would induce forces and torques on the robot’s joints. If the joints did not 
resist these forces, a motion would occur. This is the key idea behind mapping 
artificial forces in the workspace to motions of the robot arm. Therefore, we 
now derive the relationship between forces applied to the robot arm, and the 
resulting forces and torques that are induced on the robot joints. 
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Let F denote the vector of joint torques (for revolute joints) and forces (for 
prismatic joints) induced by the workspace force. As we will describe in Chapter 
[0] the principle of virtual work can be used to derive the relationship between T 
and F. Let ( Sx,Sy,5z) T be a virtual displacement in the workspace and let Sq 
be a virtual displacement of the robot’s joints. Then, recalling that work is the 
inner product of force and displacement, by applying the principle of virtual 
work we obtain 


T ■ (Sx, Sy, Sz) = F ■ Sq 


which can be written as 


T t {Sx, Sy , Sz) T = F T 5q 
Now, recall from Chapter |5.6|that 


(5.16) 


(5.17) 


Sx 

5y 

5z 


= JSq 


in which J is the Jacobian of the forward kinematic map for linear velocity (i.e. , 
the top three rows of the manipulator Jacobian). Substituting this into ( 5.16| ) 
we obtain 

F T J8q = F T 8q (5.18) 

and since this must hold for all virtual displacements Sq , we obtain 

T t J=F t (5.19) 

which implies that 

J t T = F (5.20) 

Thus we see that one can easily map forces in the workspace to joint forces and 


torques using (5.201. 


Example 5.3 A Force Acting on a Vertex of a Polygonal Robot. 

Consider the polygonal robot shown in Figure \5.6\ The vertex a has coordi- 
nates ( a x ,a y ) T in the robot’s local coordinate frame. Therefore, if the robot’s 
configuration is given by q = (x,y,9), the forward kinematic map for vertex a 
(i.e., the mapping from q = ( x , y, 9) to the global coordinates of the vertex a) is 
given by 


x + a x cos 9 — a y sin ( 
y + a x sin 9 + a y cos l 


a{x,y,9) = 

The corresponding Jacobian matrix is given by 
J a {x,y,9) = 


1 0 — a x sin# — a y cos ( 


0 1 


a x cos 9 — a y sin ( 


(5.21) 


(5.22) 
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T 



Fig. 5.6 The robot A, with coordinate frame oriented at angle 9 from the world frame, 
and vertex a with local coordinates ( a x , a y ) 


Therefore, the configuration space force is given by 


' F x ' 


1 

0 


l 1 

kV 

1 1 

Fy 

= 

0 

1 


Fg 


— a x sin 8 — a y cos 8 

a x cos 8 — a y sin 8 



T 

J X 

T 

J-y 

—T x (a x sin 6 — a y cos 9) + F y {a x cos 8 — a y sin 8) 


(5.23) 


and Fg corresponds to the torque exerted about the origin of the robot frame. 

In this simple case, one can use basic physics to arrive at the same result. In 
particular, recall that a force, T, exerted at point, a, produces a torque, t, about 
the point Oa, and this torque is given by the relationship r = r x T , in which 
r is the vector from Oa to a. Of course we must express all vectors relative 
to a common frame, and in three dimensions (since torque will be defined as 
a vector perpendicular to the plane in which the force acts). If we choose the 
world frame as our frame of reference, then we have 


a x cos 9 — a y sin 8 
a x sin 9 + a y cos 8 


0 


and the cross product gives 


t = r x T 


a x cos 9 — a y sin 8 


' T x ' 

a x sin 8 + a y cos 8 

X 

Ty 

0 


0 


0 

0 

—F x (cl x sin 8 — a y cos 9) + T y (a x cos 9 — a y sin 8) 


(5.24) 
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Thus we see that the more general expression J T T = F gives the same value 
for torque as the expression r = r x T from mechanics, o 


Example 5.4 Two-link Planar Arm 

Consider a two-link planar arm with the usual DH frame assignment. If we 
assign the control points as the origins of the DH frames ( excluding the base 
frame), the forward kinematic equations for the arm give 


[ ai(6i,02) 02 {0i , 02 ) 


h cos 0\ l\ cos + I 2 cos($i + 02 ) 
h sin 0\ li sin + Z 2 sin(0i + Bf) 


in which U are the link lengths (we use U rather than aj to avoid confusion of 
link lengths and control points). For the problem of motion planning, we require 
only the Jacobian that maps joint velocities to linear velocities, 


x 

V 



( 5 . 25 ) 


For the two-link arm, The Jacobian matrix for 02 is merely the Jacobian that 
we derived in Chapter^ 


Ja 2 (9 i) ^ 2 ) 


— aiSi — 02S\2 — (I 2 S 12 

OiCi + CI2C12 a 2 c 12 


( 5 . 26 ) 


The Jacobian matrix for a 1 is similar, but takes into account that motion of 
joint two does not affect the velocity of a±, 


Ja i($i, # 2 ) 


— a\S\ 0 
a±Ci 0 


( 5 . 27 ) 


o 

The total configuration space force acting on the robot is the sum of the 
configuration space forces that result from all attractive and repulsive control 
points 


F(q) — YF Mi (q) + F rep? -(g) 

i i 

= ( 5 - 28 ) 
i i 

in which Ji{q) is the Jacobian matrix for control point a^. It is essential that the 
addition of forces be done in the configuration space and not in the workspace. 
For example, Figure |5T7| shows a case where two workspace forces, T\ and T 2 , 
act on opposite corners of a rectang. It is easy to see that T\ + T 2 = 0, but 
that the combination of these forces produces a pure torque about the center 
of the square. 

Example 5.5 Two-link planar arm revisited. Consider again the two-link 
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Fig. 5.7 This example illustrates why forces must be mapped to the configuration 
space before they are added. The two forces illustrated in the figure are vectors of equal 
magnitude in opposite directions. Vector addition of these two forces produces zero net 
force, but there is a net moment induced by these forces 


planar arm. Suppose that that the workspace repulsive forces are given by 
^Tcp,i{0i, 0 2 ) = [7F x ,i, Fy,i} T ■ For the two-link planar arm , the repulsive forces 
in the configuration space are then given by 


Frep(q) = 


— aiSi 

0 


aiCi 

0 



Fx, 1 


1 

1 


— aiSi — &2S12 
~a 2 s 12 


aiCi + a 2 ci2 
«2Cl2 



F x .2 


. T yv . 


(5.29) 


o 


5.3.3 Motion Planning Algorithm 


Having defined a configuration space force, we can use the same gradient descent 
method for this case as in Section 5.3 As before, there are a number of design 
choices that must be made. 


controls the relative influence of the attractive potential for control point Gq. 
It is not necessary that all of the Q be set to the same value. Typically, we 
weight one of the control points more heavily than the others, producing 
a “follow the leader” type of motion, in which the leader control point is 
quickly attracted to its final position, and the robot then reorients itself 
so that the other attractive control points reach their final positions. 

r/j controls the relative influence of the repulsive potential for control point aj. 
As with the Q it is not necessary that all of the r/j be set to the same 
value. In particular, we typically set the value of r/j to be much smaller 
for obstacles that are near the goal position of the robot (to avoid having 
these obstacles repel the robot from the goal). 

po As with the r/j , we can define a distinct po for each obstacle. In particular, we 
do not want any obstacle’s region of influence to include the goal position 
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of any repulsive control point. We may also wish to assign distinct po’s 
to the obstacles to avoid the possibility of overlapping regions of influence 
for distinct obstacles. 


5.4 USING RANDOM MOTIONS TO ESCAPE LOCAL MINIMA 


As noted above, one problem that plagues artificial potential field methods 
for path planning is the existence of local minima in the potential field. In 
the case of articulated manipulators, the resultant held U is the sum of many 
attractive and repulsive fields defined over -ft 3 . This problem has long been 
known in the optimization community, where probabilistic methods such as 
simulated annealing have been developed to cope with it. Similarly, the robot 
path planning community has developed what are known as randomized methods 
to deal with this and other problems. The first of these methods was developed 
specifically to cope with the problem of local minima in potential fields. 

The first planner to use randomization to escape local minima was called 
RPP (for Randomized Potential Planner). The basic approach is straightfor- 
ward: use gradient descent until the planner finds itself stuck in a local mini- 
mum, then use a random walk to escape the local minimum. The algorithm is 
a slight modification of the gradient descent algorithm of Section |5.3| 


1 . 

2 . 


3. 

4. 


q° <- 4init> i <- 0 
IF <f ± 9final 

q l+1 <— q l + a 1 


p(q l ) 

IRWDII 


i <— i + 1 

ELSE return < q°, q 1 ■ ■ ■ q i > 
IF stuck in a local minimum 


execute a random walk, ending at q' 


q i+1 . 

GO TO 2 


The two new problems that must be solved are determining when the planner 
is stuck in a local minimum and defining the random walk. Typically, a heuristic 
is used to recognize a local minimum. For example, if several successive q 1 
lie within a small region of the configuration space, it is likely that there is a 
nearby local minimum (e.g., if for some small positive e we have ||g* — g* +1 || < e, 

1 1 g* — q l+2 1| < e, and \\q' 1 — g l+3 || < e then assume q l is near a local minimum). 

Defining the random walk requires a bit more care. The original approach 
used in RPP is as follows. The random walk consists of t random steps. A 
random step from q= (qi, ■ ■ ■ q n ) is obtained by randomly adding a small fixed 
constant to each q it 


Q random— step 


(<ll ±Vl -q n ± V n ) 
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with Vi a fixed small constant and the probability of adding +Vi or —Vi equal to 
1/2 (i.e. , a uniform distribution). Without loss of generality, assume that q = 0. 
We can use probability theory to characterize the behavior of the random walk 
consisting of t random steps. In particular, the probability density function for 
q' = (<?!,■ • • ,q n ) is given by 


Pi(q%,t ) 


Vi y/2wt 


exp 



(5.30) 


which is a zero mean Gaussian density function with variance vft,. This is a 
result of the fact that the sum of a set of uniformly distributed random variables 
is a Gaussian random variable^ The variance vft essentially determines the 
range of the random walk. If certain characteristics of local minima (e.g., the 
size of the basin of attraction) are known in advance, these can be used to 
select the parameters v t and t. Otherwise, they can be determined empirically, 
or based on weak assumptions about the potential field (the latter approach 
was used in the original RPP). 


5.5 PROBABILISTIC ROADMAP METHODS 

The potential field approaches described above incrementally explore Qf ree) 
searching for a path from g init to g final . At termination, these planners re- 
turn a single path. Thus, if multiple path planning problems must be solved, 
such a planner must be applied once for each problem. An alternative approach 
is to construct a representation of Qf ree that can be used to quickly generate 
paths when new path planning problems arise. This is useful, for example, 
when a robot operates for a prolonged period in a single workspace. 

In this section, we will describe probabilistic roadmaps (PRMs), which are 
one-dimensional roadmaps in <2f ree that can be used to quickly generate paths. 
Once a PRM has been constructed, the path planning problem is reduced to 
finding paths to connect q init and <7fi na i to the roadmap (a problem that is 
typically much easier than finding a path from q init to (/ final ). 

A PRM is a network of simple curve segments, or arcs, that meet at nodes. 
Each node corresponds to a configuration. Each arc between two nodes cor- 
responds to a collision free path between two configurations. Constructing a 
PRM is a conceptually straightforward process. First, a set of random configu- 
rations is generated to serve as the nodes in the network. Then, a simple, local 
path planner is used to generate paths that connect pairs of configurations. 


2 A Gaussian density function is the classical bell shaped curve. The mean indicates the 
center of the curve (the peak of the bell) and the variance indicates the width of the bell. 
The probability density function (pdf) tells how likely it is that the variable <j; will lie in a 
certain interval. The higher the pdf values, the more likely that qi will lie in the corresponding 
interval. 
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(c) (d) 

Fig. 5.8 (a) A two-dimensional configuration space populated with several random sam- 
ples (b) One possible PRM for the given configuration space and random samples (c) 
PRM after enhancement (d) path from g init to 9 final found by connecting g init and g flnal 
to the roadmap and then searching the roadmap for a path from q init to g flnal 


Finally, if the initial network consists of multiple connected component^] it is 
augmented by an enhancement phase, in which new nodes and arcs are added 
in an attempt to connect disjoint components of the network. To solve a path 
planning problem, the simple, local planner is used to connect g init and 
to the roadmap, and the resulting network is searched for a path from g init to 
c/ final . These four steps are illustrated in Figure 5.8 We now discuss these steps 
in more detail. 


3 A connected component is a maximal subnetwork of the network such that a path exists in 
the subnetwork between any two nodes. 
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2-norm in C-space: 
oo-norm in C-space: 

2-norm in workspace: 
oo-norm in workspace: 


EIU (s' -ft) 2 


\W~q\\ = 

max„ \q[ - qi\ 

£3 P e^l|p(9 / ) -p(g)| 


1 

2 


max p6 ^||p(g / ) -p(g)| 


Table 5.1 Four commonly used distance functions 


5.5.1 Sampling the configuration space 

The simplest way to generate sample configurations is to sample the configu- 
ration space uniformly at random. Sample configurations that lie in QO are 
discarded. A simple collision checking algorithm can determine when this is the 
case. The disadvantage of this approach is that the number of samples it places 
in any particular region of Qf ree is proportional to the volume of the region. 
Therefore, uniform sampling is unlikely to place samples in narrow passages of 
Qfree- In the PRM literature, this is refered to as the narrow passage prob- 
lem. It can be dealt with either by using more intelligent sampling schemes, or 
by using an enhancement phase during the construction of the PRM. In this 
section, we discuss the latter option. 

5.5.2 Connecting Pairs of Configurations 

Given a set of nodes that correspond to configurations, the next step in building 
the PRM is to determine which pairs of nodes should be connected by a simple 
path. The typical approach is to attempt to connect each node to it’s k nearest 
neighbors, with k a parameter chosen by the user. Of course, to define the 
nearest neighbors, a distance function is required. Table [5~T| lists four distance 
functions that have been popular in the PRM literature. For the equations in 
this table, the robot has n joints, q and q' are the two configurations corre- 
sponding to different nodes in the roadmap, q t refers to the configuration of the 
ith joint, and p (q) refers to the workspace reference point p of a set of reference 
points of the robot, A , at configuration q. Of these, the simplest, and perhaps 
most commonly used, is the 2-norm in configuraiton space. 

Once pairs of neighboring nodes have been identified, a simple local planner is 
used to connect these nodes. Often, a straight line in configuration space is used 
as the candidate plan, and thus, planning the path between two nodes is reduced 
to collision checking along a straight line path in the configuration space. If 
a collision occurs on this path, it can be discarded, or a more sophisticated 
planner (e.g., RPP discussed above) can be used to attempt to connect the 
nodes. 
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The simplest approach to collision detection along the straight line path is to 
sample the path at a sufficiently fine discretization, and to check each sample 
for collision. This method works, provided the discretization is fine enough, but 
it is terribly inefficient. This is because many of the computations required to 
check for collision at one sample are repeated for the next sample (assuming 
that the robot has moved only a small amount between the two configurations). 
For this reason, incremental collision detection approaches have been developed. 
While these approaches are beyond the scope of this text, a number of collision 
detection software packages are available in the public domain. Most developers 
of robot motion planners use one of these packages, rather than implementing 
their own collision detection routines. 

5.5.3 Enhancement 

After the initial PRM has been constructed, it is likely that it will consist of 
multiple connected components. Often these individual components lie in large 
regions of Qf ree that are connected by narrow passages in Q f ree . The goal of 
the enhancement process is to connect as many of these disjoint components as 
possible. 

One approach to enhancement is to merely attempt to directly connect nodes 
in two disjoint components, perhaps by using a more sophisticated planner such 
as RPP. A common approach is to identify the largest connected component, 
and to attempt to connect the smaller components to it. The node in the 
smaller component that is closest to the larger component is typically chosen as 
the candidate for connection. A second approach is to choose a node randomly 
as candidate for connection, and to bias the random choice based on the number 
of neighbors of the node; a node with fewer neighbors in the network is more 
likely to be near a narrow passage, and should be a more likely candidate for 
connection. 

A second approach to enhancement is to add samples more random nodes to 
the PRM, in the hope of finding nodes that lie in or near the narrow passages. 
One approach is to identify nodes that have few neighbors, and to generate 
sample configurations in regions around these nodes. The local planner is then 
used to attempt to connect these new configurations to the network. 

5.5.4 Path Smoothing 

After the PRM has been generated, path planning amounts to connecting q init 
and to the network using the local planner, and then performing path 

smoothing, since the resulting path will be composed of straight line segments 
in the configuration space. The simplest path smoothing algorithm is to select 
two random points on the path and try to connect them with the local planner. 
This process is repeated until until no significant progress is made. 
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5.6 TRAJECTORY PLANNING 

A path from q init to qfi na i is defined as a continuous map, r : [0, 1] — > Q, with 
t(0) = q init and r(l) = qf ina i . A trajectory is a function of time q(t) such 
that q(to) = q init and q(tf) = qfi na i ■ In this case, tf —to represents the amount 
of time taken to execute the trajectory. Since the trajectory is parameterized 
by time, we can compute velocities and accelerations along the trajectories by 
differentiation. If we think of the argument to r as a time variable, then a 
path is a special case of a trajectory, one that will be executed in one unit of 
time. In other words, in this case r gives a complete specification of the robot’s 
trajectory, including the time derivatives (since one need only differentiate r to 
obtain these). 

As seen above, a path planning algorithm will not typically give the map 
t; it will give only a sequence of points (called via points) along the path. 
Further, there are other ways that the path could be specified. In some cases, 
paths are specified by giving a sequence of end-effector poses, Tg(kAt). In this 
case, the inverse kinematic solution must be used to convert this to a sequence 
of joint configurations. A common way to specify paths for industrial robots is 
to physically lead the robot through the desired motion with a teach pendant, 
the so-called teach and playback mode. In some cases, this may be more 
efficient than deploying a path planning system, e.g. in environments such as 
the one shown in Figure |5.9| In this case, there is no need for calculation of 
the inverse kinematics. The desired motion is simply recorded as a set of joint 
angles (actually as a set of encoder values) and the robot can be controlled 
entirely in joint space. Finally, in cases for which no obstacles are present, the 
manipulator is essentially unconstrained. It is often the case that a manipulator 
motion can be decomposed into a segments consisting of free and guarded 
motions, shown in Figure |5.10[ During the free motion, the manipulator can 
move very fast, since no obstacles are near by, but at the start and end of the 
motion, care must be taken to avoid obstacles. 

We first consider point to point motion. In this case the task is to plan a 
trajectory from q(to) to q(tf ), i.e. , the path is specified by its initial and final 
configurations. In some cases, there may be constraints on the trajectory (e.g., 
if the robot must start and end with zero velocity). Nevertheless, it is easy to 
realize that there are infinitely many trajectories that will satisfy a finite number 
of constraints on the endpoints. It is common practice therefore to choose 
trajectories from a finitely parameterizable family, for example, polynomials of 
degree n, with n dependant on the number of constraints to be satisfied. This is 
the approach that we will take in this text. Once we have seen how to construct 
trajectories between two configurations, it is straightforward to generalize the 
method to the case of trajectories specified by multiple via points. 
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Final 

configuration 


Fig. 5.9 Via points to plan motion around obstacles 



l \ 

guarded 


Fig. 5.10 Guarded and free motions 
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5.6.1 Trajectories for Point to Point Motion 

As described above, the problem here is to find a trajectory that connects an 
initial to a final configuration while satisfying other specified constraints at 
the endpoints (e.g., velocity and/or acceleration constraints). Without loss of 
generality, we will consider planning the trajectory for a single joint, since the 
trajectories for the remaining joints will be created independently and in exactly 
the same way. Thus, we will concern ourselves with the problem of determining 
g(f), where q(t) is a scalar joint variable. 

We suppose that at time to the joint variable satisfies 


q{to) = 

9o 

(5.31) 

q(to) = 

Vo 

(5.32) 

and we wish to attain the values at tf 

q(tf) = 

9/ 

(5.33) 

4(t f ) = 

Vf 

(5.34) 


Figure |5.11| shows a suitable trajectory for this motion. In addition, we may 
wish to specify the constraints on initial and final accelerations. In this case we 
have two the additional equations 


q(to) = u 0 (5.35) 

q(t f ) = a f (5.36) 


5.6. 1.1 Cubic Polynomial Trajectories Suppose that we wish to generate a tra- 
jectory between two configurations, and that we wish to specify the start and 
end velocities for the trajectory. One way to generate a smooth curve such as 
that shown in Figure |5.11| is by a polynomial function of t. If we have four 
constraints to satisfy, such as ( 5.31 )-( 5.33 ) , we require a polynomial with four 
independent coefficients that can be chosen to satisfy these constraints. Thus 
we consider a cubic trajectory of the form 


q(t) = a 0 + 

Then the desired velocity is given as 

q(t) = ai 


0\t + 0-2^2 H - ^3^ 

(5.37) 

“h 2(2-2^ “1“ 3(23^ 

(5.38) 


Combining equations (5.37 1 and (5.38) with the four constraints yields four 
equations in four unknowns 


9o 

= Oq + + a 3^0 

(5.39) 

v 0 

= cl\ + 2a 2 £o “I - 3a3^Q 

(5.40) 

9/ 

= flo + CLi tf + 0,2 tf + 

(5.41) 

Vf 

= d\ ~\~ 2(2-2 tf H - 3g&3 tf 

(5.42) 
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Fig. 5.11 Typical Joint Space Trajectory 


These four equations can be combined into a single matrix equation 


1 

t f 

1 


t 2 

l o 

2to 

t f 

2t f 


4 

3 4 

t3 f 

3 4 



a 0 


<70 


a\ 


VO 


02 


If 


. °3 . 


. V f . 


(5.43) 


It can be sh own ( Problem [5|l| that the determinant of the c oeffic ient matrix 
in Equation (5.431 is equal to ( tf — ffi) 4 and, hence, Equation (5.431 always has 
a unique solution provided a nonzero time interval is allowed for the execution 
of the trajectory. 


Example 5.6 


Writing Equation (5.43) as 


Ma = b 


(5.44) 


where M is the coefficient matrix, a = [cio, a±, ci 2 , a^] T is the vector of coeffi- 
cients of the cubic polynomial, and b = [go, vq, qi, v{\ T is the vector of initial 
data (initial and final positions and velocities) , the Matlab script shown in Fig- 
ure \5.1S\ computes the general solution as 

a = M~ x b (5.45) 
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7.7. 

7.7. cubic.m 

7.7. 

7.7. M-file to compute a cubic polynomial reference trajectory 


7.7. 


7.7. qO 

= initial position 

7.7. vO 

= initial velocity 

7.7. ql 

= final position 

7.7. vl 

= final velocity 

7.7. to 

= initial time 

7.7. tf 

= final time 

7.7. 


clear 



d = input ( ’ initial data = [qO , vO ,ql , vl ,t0, tf ] = ’) 
qO = d(l) ; vO = d(2); ql = d(3) ; vl = d(4) ; 
tO = d(5) ; tf = d(6) ; 
t = linspace(tO , tf , 100* (tf-tO) ) ; 
c = ones (size (t) ) ; 

M = [ 1 tO t0'2 t0"3; 

0 1 2*t0 3*t0~2 ; 

1 tf tf “2 tf~3; 

0 1 2*tf 3*tf~2] ; 

7.7. 

b = [qO; vO; ql ; vl] ; 
a = inv(M)*b; 

7.7. 

7. qd = reference position trajectory 
7. vd = reference velocity trajectory 
7. ad = reference acceleration trajectory 

7. 

qd = a(l).*c + a(2).*t +a(3).*t.~2 + a(4).*t.~3; 
vd = a(2).*c +2*a(3).*t +3*a(4) . *t . ~2 ; 
ad = 2*a(3).*c + 6*a(4).*t; 


Fig. 5.12 Matlab code for Example 1 5. 6 1 
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As an illustrative example, we may consider the special case that the initial 
and final velocities are zero. Suppose we take to = 0 and tf = 1 sec, with 

v o = 0 Vf = 0 (5.46) 


Thus we want to move from the initial position qo to the final position qf in 
1 second, starting and ending with zero velocity. From the Equation ( 5.f3 ) we 
obtain 


' 1 0 0 0' 


ao 


Qo 

0 10 0 


at 


0 

1111 


02 


<?/ 

0 12 3 


. ®3 . 


0 


This is then equivalent to the four equations 


a 0 = q o (5.48) 

ai = 0 (5.49) 

a 2 + a 3 = q f - q 0 (5.50) 

2 a 2 + 3 a 3 = 0 (5.51) 

These latter two can be solved to yield 

«2 = 3(q f -qo) (5.52) 

a 3 = -2 (q f -q 0 ) (5.53) 


The required cubic polynomial function is therefore 


Qi(t) = qo + 3(<?/ - q 0 )t 2 - 2(q f - q 0 )t 3 


(5.54) 


Figure 5.1fjf a) shows this trajectory with qo = 10°, qf = — 20 °. The cor- 
responding velocity and acceleration curves are given in Figures 5. 1 3]f b ) and 

JJ^c). ' 




Fig. 5.13 (a) Cubic polynomial trajectory (b) Velocity profile for cubic polynomial 

trajectory (c) Acceleration profile for cubic polynomial trajectory 


O 
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5.6. 1.2 Quintic Polynomial Trajectories As can be seein in Figure [5. 13[ a cubic 
trajectory gives continuous positions and velocities at the start and finish points 
times but discontinuities in the acceleration. The derivative of acceleration is 
called the jerk. A discontinuity in acceleration leads to an impulsive jerk, which 
may excite vibrational modes in the manipulator and reduce tracking accuracy. 
For this reason, one may wish to specify constraints on the acceleration as well 
as on the position and velocity. In this case, we have six constraints (one each 
for initial and final configurations, initial and final velocities, and initial and 
final accelerations). Therefore we require a fifth order polynomial 


q(t) = a 0 + ait A a 2 t 2 + a 3 t 3 A ayt A A a 5 t 5 


(5.55) 


Using (5.311 - ( 5.36 1 and taking the appropriate number of derivatives we obtain 
the following equations, 


<Zo — a o A aif 0 + 02^0 A °3^o A a &4 A a 5^0 
Vo = a± A 2a 2 to A 3a 3 4 + 4.0^4 + 5a 3 4 
ot 0 = 2a 2 A da 3 to A 12 u 4 tg A 20a 3 4 

q f = b a i tf T a 2 t 2 A 03 t 3 A 04 ty A ayt 3 ^ 
Vf = a 3 + 2a 2 t f + 3a 3 t 2 f + 4a^t 3 + 5a 3 t"f 
Oif = 2a 2 A 6a 3 tf A 12a^t 2 - A 20a 3 t 3 

which can be written as 



' 1 

to 

/ 2 

l 0 

4 

4 

*0 


a 0 


<?o 


0 

1 

2 to 

3 4 

4 4 

5 4 


ai 


*'o 


0 

0 

2 

6 to 

12 4 

20t 3 


02 


a 0 


1 

t t 

t 2 

T f 

4 

4 

4 


03 


9/ 


0 

1 

2 tf 

3tj 

4 4 

5 4 


a4 


Vf 


0 

0 

2 

6 tf 

12 4 

20 4 \ 


05 


. a f 

. 14| shows 

the Matlab 

script 

that gives the 

general so 


(5.56) 


equation. 


Example 5.8 

Figure 5.15 shows a quintic polynomial trajectory with ^(O) 
with zero initial and jinal velocities and accelerations, 
o 


0, q( 2) = 40 


5.6. 1.3 Linear Segments with Parabolic Blends (LSPB) Another way to gen- 
erate suitable joint space trajectories is by so-called Linear Segments with 
Parabolic Blends or (LSPB) for short. This type of trajectory is appropri- 
ate when a constant velocity is desired along a portion of the path. The LSPB 
trajectory is such that the velocity is initially “ramped up” to its desired value 
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7.7. 

7.7. quint ic.m 

&& 


7.7. M-file to compute a quintic polynomial reference trajectory 

7.7. 

7.7. qO = initial position 

7.7. vO = initial velocity 

7.7. acO = initial acceleration 

7.7. ql = final position 

7.7. vl = final velocity 

7.7. acl = final acceleration 

7.7. tO = initial time 

7.7. tf = final time 

7.7. 


clear 


d = input ( ’ initial data = [qO , vO, acO ,ql , vl , acl , tO , tf ] = ’) 

qO = d(l); vO = d(2); acO = d (3) ; 

ql = d(4); vl = d(5); acl = d (6) ; 

tO = d(7) ; tf = d(8) ; 

t = linspace(tO , tf , 100* (tf-tO) ) ; 

c = ones (size (t) ) ; 

M = [ 1 tO t0'2 t0~3 t0~4 t0~5; 

0 1 2*t0 3*t0~2 4*t0~3 5*t0~4; 

002 6*t0 12*t0~2 20*t0~3 ; 

1 tf tf ~2 tf ~3 tf "4 tf "5 ; 

0 1 2*tf 3*tf ~2 4*tf ~3 5*tf ~4; 

002 6*tf 12*tf ~2 20*tf ~3] ; 

7.7. 

b=[qO; vO; acO; ql ; vl ; acl]; 

a = inv(M)*b; 

7.7. 

7.7. qd = position trajectory 

7.7. vd = velocity trajectory 

7.7. ad = acceleration trajectory 

7.7. 

qd = a(l).*c + a(2).*t +a(3).*t.~2 + a(4).*t.~3 +a(5).*t.~4 + a(6).*t."5; 

vd = a(2).*c +2*a(3).*t +3*a(4) . *t . ~2 +4*a(5) . *t . ~3 +5*a(6) . *t . ~4; 

ad = 2*a(3).*c + 6*a(4).*t +12*a(5) . *t . ~2 +20*a(6) . *t . ~3 ; 


Fig. 5.14 Matlab code to generate coefficients for quintic trajectory segment 
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(a) (b) (c) 

Fig. 5.15 (a) Quintic Polynomial Trajectory, (b) Velocity Profile for Quintic Polyno- 

mial Trajectory, (c) Acceleration Profile for Quintic Polynomial Trajectory 


and then “ramped down” when it approaches the goal position. To achieve this 
we specify the desired trajectory in three parts. The first part from time to to 
time tf, is a quadratic polynomial. This results in a linear “ramp” velocity. At 
time tb, called the blend time, the trajectory switches to a linear function. 
This corresponds to a constant velocity. Finally, at time tf — tb the trajectory 
switches once again, this time to a quadratic polynomial so that the velocity is 
linear. 


Blend Times for LSPB Trajectory 



Time (sec) 


tig. s.io menu tunes tor 






We choose the blend time tb so that the position curve is symmetric as shown 


in Figure 5.16 For convenience suppose that to = 0 and q{tf) = 0 = q( 0). Then 
between times 0 and tb we have 


q(t) = a 0 + ait + a 2 t 2 


(5.57) 


so that the velocity is 


<i(t) = 


CL\ + 


(5.58) 
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The constraints qo = 0 and q{ 0) = 0 imply that 



a 0 

= qo 

(5.59) 


ai 

= 0 

(5.60) 

At time ti, we want the velocity to equal a given constant, say V. 

Thus, we 

have 

4{tb) = 

= 2a2 tb = V 

(5.61) 

which implies that 

<22 

V 

2 Tb 

(5.62) 

Therefore the required trajectory between 0 and tb is given as 



q{t) = 

V 2 

; ,0 + 2 S' 

(5.63) 


= 

(X, o 

: qo+ 2 t ~ 



q(t) = 

V 

- — t = at 

tb 

(5.64) 


q = 

V 

- — = a 

tb 

(5.65) 

where a denotes the acceleration. 



Now, between time tf 

and tf — tb, the trajectory is a linear segment (corre- 

sponding to a constant velocity V) 




c[(i) — clq + d\t — clq + Vt 

(5.66) 

Since, by symmetry, 

<i) 

qo + qj 

2 

(5.67) 

we have 

qo + qj 

2 

= a a + V E 

(5.68) 

which implies that 

a 0 = 

qo + qf - Vt f 

(5.69) 


2 

Since the two segments must “blend’ 

’ at time tb we require 



V 

q o + h 


go + qj - Vtf 


+ Vt-b 


2 


(5.70) 
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(a) 


(b) 


(c) 


Fig. 5.17 (a) LSPB trajectory (b) Velocity profile for LSPB trajectory (c) Acceleration 

for LSPB trajectory 


which gives upon solving for the blend time tb 

go ~Qf + Vt f 


h = 


V 


(5.71) 


Note that we have the constraint 0 < if, < -f- This leads to the inequality 


g / - go , < 2 (g / - go) 

V f ~ V 

To put it another way we have the inequality 

g/ - g° < y < 2 (g / - go) 




tf 


(5.72) 


(5.73) 


Thus the specified velocity must be between these limits or the motion is not 
possible. 

The portion of the trajectory between tf—tb and tf is now found by symmetry 
considerations (Problem [5|6]) . The complete LSPB trajectory is given by 


g(t) = < 


go + 7,t 


g/ + go - Vtf 


Vt 


0 < t < t b 


tb < t < t f — tb 


(5.74) 


at t a Q 

g/ 2 + at ft — —t 2 t f -t b <t<tf 


Figure 5.17[ a) shows such an LSPB trajectory, where the maximum velocity 
V = 60. In this case t b = |. The velocity and acceleration curves are given in 
Figures 5.17[b) and 5.17 ’c), respectively. 


5.6. 1.4 Minimum Time Trajectories An important variation of this trajectory 
is obtained by leaving the final time tf unspecified and seeking the “fastest” 
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trajectory between go and g/ with a given constant acceleration a, that is, the 
trajectory with the final time tf a minimum. This is sometimes called a Bang- 
Bang trajectory since the optimal solution is achieved with the acceleration at 
its maximum value +a until an appropriate switching time t s at which time 
it abruptly switches to its minimum value — a (maximum deceleration) from t s 
to tf. 

Returning to our simple example in which we assume that the trajectory 
begins and ends at rest, that is, with zero initial and final velocities, symmetry 
considerations would suggest that the switching time t s is just (/. This is 
indeed the case. For nonzero initial and/or final velocities, the situation is 
more complicated and we will not discuss it here. 

If we let V s denote the velocity at time t s then we have 

V s = at s (5.75) 


and also 


t s = 


go - Qf + Vstf 

V„ 


The symmetry condition t s = // implies that 

V s = 

ts 

Combining these two we have the conditions 

9/ -9o 


ts 


= at. 


which implies that 


ts = 


'9/ ~ 9o 
a 


(5.76) 


(5.77) 


(5.78) 


(5.79) 


5.6.2 Trajectories for Paths Specified by Via Points 

Now that we have examined the problem of planning a trajectory between two 
configuration, we generalize our approach to the case of planning a trajectory 
that passes through a sequence of configurations, called via points. Consider 
the simple of example of a path specified by three points, go , gi , 92 , such that 
the via points are reached at times to,ti an d 1 2 . If in addition to these three 
constraints we impose constraints on the initial and final velocities and accel- 
erations, we obtain the following set of constraints, 
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q(to) 

= 9o 

q(to) 

= ^0 

q(to) 

= c*0 

q(t i) 

= 9i 

q(t 2 ) 

= 92 

q(t 2 ) 

= V 2 

q{h) 

= 02 


which could be satisfied by generating a trajectory using the sixth order poly- 
nomial 

q(t) = ae t 6 + a$t 5 + a±t A + 03 t 3 + a 2 t 2 + ait 1 + ao (5.80) 

One advantage to this approach is that, since q(t ) is continuously differen- 
tiable, we need not worry about discontinuities in either velocity or acceleration 
at the via point, q±. However, to determine the coefficients for this polynomial, 
we must solve a linear system of dimension seven. The clear disadvantage to 
this approach is that as the number of via points increases, the dimension of 
the corresponding linear system also increases, making the method intractable 
when many via points are used. 

An alternative to using a single high order polynomial for the entire trajec- 
tory is to use low order polynomials for trajectory segments between adjacent 
via points. These polynomials sometimes refered to as interpolating polyno- 
mials or blending polynomials. With this approach, we must take care that 
continuity constraints (e.g., in velocity and acceleration) are satisfied at the via 
points, where we switch from one polynomial to another. 

Given initial and final times, tg and tf, respectively, with 


q d {t 0 ) = q 0 ; q d (t f ) = qi 

Qd(to) = q' 0 ; = q'i 


(5.81) 


the required cubic polynomial q d (t) can be computed from 

q d (t Q ) = a 0 + ai{t - t 0 ) + a 2 (t - t 0 ) 2 + a 3 (t - t 0 ) 3 (5.82) 


where 

_ 3(gi ^ go) ^ (2 q' 0 + q'i){t f - t 0 ) _ 2 (q 0 - qi ) + (q' 0 + q'^jtf - t 0 ) 

02 (tf-to) 2 “ 3 (tf-to) 3 

A sequence of moves can be planned using the above formula by using the 
end conditions qf, Vf of the i-th move as initial conditions for the * + 1-st move. 


Example 5.9 Figure \5.1£\ shows a 6-second, move, computed in three parts 
using (5.82), where the trajectory begins at 10° and is required to reach 40° at 
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(a) 




(b) 


(c) 


Fig. 5.18 (a) Cubic spline trajectory made from three cubic polynomials (b) Velocity 

Profile for Multiple Cubic Polynomial Trajectory (c) Acceleration Profile for Multiple 
Cubic Polynomial Trajectory 



Fig. 5.19 (a) Trajectory with Multiple Quintic Segments (b) Velocity Profile for Mul- 

tiple Quintic Segments (c) Acceleration Profile for Multiple Quintic Segments 


2-seconds, 30 ° at 4 seconds , and 90 ° at 6-seconds, with zero velocity at 0,2,4 > 
and 6 seconds, 
o 

Example 5.10 Figure [ 5 . 1 9\ shows the same six second trajectory as in Exam- 
ple | r >. 9\ with the added constraints that the accelerations should be zero at the 
blend times, 
o 


5.7 HISTORICAL PERSPECTIVE 

The earliest work on robot planning was done in the late sixties and early sev- 
enties in a few University-based Artificial Intelligence (AI) labs [25; [SI I5T . 
This research dealt with high level planning using symbolic reasoning that was 
much in vogue at the time in the AI community. Geometry was not often 
explicitly considered in early robot planners, in part because it was not clear 
how to represent geometric constraints in a computationally plausible manner. 
The configuration space and its application to path planning were introduced in 
m- This was the first rigorous, formal treatment of the geometric path plan- 
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ning problem, and it initiated a surge in path planning research. The earliest 
work in geometric path planning developed methods to construct volumetric 
representations of the free configuration space. These included exact methods 
(e.g., [65]). and approximate methods (e.g., mmm)- In the former case, 
the best known algorithms have exponential complexity and require exact de- 
scriptions of both the robot and its environment, while in the latter case, the 
size of the representation of C-space grows exponentially in the dimension of 
the C-space. The best known algorithm for the path planning problem, giving 
an upper bound on the amount of computation time required to solve the prob- 
lem, appeared in m- That real robots rarely have an exact description of the 
environment, and a drive for faster planning systems led to the development of 
potential fields approaches mm- 

By the early nineties, a great deal of research had been done on the geometric 
path planning problem, and this work is nicely summarized in the textbook 
@3 ■ This textbook helped to generate a renewed interest in the path planning 
problem, and it provided a common framework in which to analyze and express 
path planning algorithms. Soon after, the research field of Algorithmic Robotics 
was born at a small workshop in San Francisco m- 

In the early nineties, randomization was introduced in the robot planning 
community [5], originally to circumvent the problems with local minima in 
potential fields). Early randomized motion planners proved effective for a large 
range of problems, but sometimes required extensive computation time for some 
robots in certain environments [38] . This limitation, together with the idea that 
a robot will operate in the same environment for a long period of time led to 
the development of the probabilistic roadmap planners 133 ESI m- 

Finally, much work has been done in the area of collision detection in recent 
years. @51 E21 E3I, 174] , This work is primarily focused on finding efficient, 
incremental methods for detecting collisions between objects when one or both 
are moving. A number of public domain collision detection software packages 
are currently available on the internet. 
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Problems 


MOTION PLANNING PROBLEMS TO BE WRITTEN 


5-1 Show by direct calculation that the determinant of the coefficient matrix 
in Equation (5.431 is (tf — to) 4 - 


5-2 Use Gaussian elimination to reduce the system (5.431 to upper triangular 


form and verify that the solution is indeed given by Equation (5.821. 


5-3 Suppose we wish a manipulator to start from an initial configuration at time 
to and track a conveyor. Discuss the steps needed in planning a suitable 
trajectory for this problem. 


5-4 Suppose we desire a joint space trajectory qf(t ) for the i-tli joint (assumed 
to be revolute) that begins at rest at position qo at time to and reaches 
position qi in 2 seconds with a final velocity of 1 radian/sec. Compute a 
cubic polynomial satisfying these constraints. Sketch the trajectory as a 
function of time. 


5-5 Compute a LSPB trajectory to satisfy the same requirements as in Prob- 
lem 5]4 Sketch the resulting position, velocity, and acceleration profiles. 


5-6 


Fill in the details of the computation of the LSPB trajectorj 
words compute the portion of the trajectory between times tf 
and hence verify Equations (5.74). 


In other 
tb and tf 


5-7 Write a Matlab m-file, lspb.m, to generate an LSPB trajectory, given ap- 
propriate initial data. 


5-8 Rewrite the Matlab m-files, cubic. m, quintic.m, and lspb.m to turn them 
into Matlab functions. Document them appropriately. 


6 

DYNAMICS 


r J. 1 his chapter deals with the dynamics of robot manipulators. Whereas the 
kinematic equations describe the motion of the robot without consideration of 
the forces and torques producing the motion, the dynamic equations explicitly 
describe the relationship between force and motion. The equations of motion 
are important to consider in the design of robots, in simulation and animation 
of robot motion, and in the design of control algorithms. We introduce the 
so-called Euler-Lagrange equations, which describe the evolution of a me- 
chanical system subject to holonomic constraints (this term is defined later 
on). To motivate the Euler-Lagrange approach we begin with a simple deriva- 
tion of these equations from Newton’s Second Law for a one-degree-of-freedom 
system. We then derive the Euler-Lagrange equations from the principle of 
virtual work in the general case. 

In order to determine the Euler-Lagrange equations in a specific situation, 
one has to form the Lagrangian of the system, which is the difference between 
the kinetic energy and the potential energy; we show how to do this in sev- 
eral commonly encountered situations. We then derive the dynamic equations 
of several example robotic manipulators, including a two-link cartesian robot, 
a two-link planar robot, and a two- link robot with remotely driven joints. 

The Euler-Lagrange equations have several very important properties that 
can be exploited to design and analyze feedback control algorithms. Among 
these are explicit bounds on the inertia matrix, linearity in the inertia param- 
eters, and the so-called skew symmetry and passivity properties. We discuss 
these properties in Section [631 
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This chapter is concluded with a derivation of an alternate the formulation of 
the dynamical equations of a robot, known as the Newton-Euler formulation 
which is a recursive formulation of the dynamic equations that is often used for 
numerical calculation. 


6.1 THE EULER-LAGRANGE EQUATIONS 

In this section we derive a general set of differential equations that describe 
the time evolution of mechanical systems subjected to holonomic constraints, 
when the constraint forces satisfy the principle of virtual work. These are called 
the Euler-Lagrange equations of motion. Note that there are at least two 
distinct ways of deriving these equations. The method presented here is based 
on the method of virtual displacements; but it is also possible to derive the 
same equations based on Hamilton’s principle of least action [?]. 

6.1.1 One Dimensional System 

To motivate the subsequent derivation, we show first how the Euler-Lagrange 
equations can be derived from Newton’s Second Law for a single degree of 
freedom system consisting of a particle of constant mass m , constrained to 
move in the y-direction, and subject to a force / and the gravitational force 
mg, as shown in Figure |6.1[ By Newton’s Second law, the equation of motion 



Fig. 6. 1 One Degree of Freedom System 


of the particle is 


my = / — mg 


Notice that the left hand side of Equation (6.1) can be written as 


d d ( 1 


m y = =m*7.\F rn y ) = T«F 


dt dy \ 2 


d dIC 


dt dy 


( 6 . 1 ) 


( 6 . 2 ) 


where K, = \my 2 is the kinetic energy. We use the partial derivative notation 
in the above expression to be consistent with systems considered later when the 
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kinetic energy will be a function of several variables. Likewise we can express 

(6.3) 


the gravitational force in Equation (6.1 1 as 


8 , , dV 

mg = g- y M = ^ 


where V = mgy is the potential energy due to gravity. If we define 


and note that 


C = K, — V = — my' 2 — mgy 


dC OK, dC _dV 

dij ~ ~di) “ dy ~ ~dy 


(6.4) 


then we can write Equation (6.1 1 as 


dt dy dy 


(6.5) 


The function £, which is the difference of the kinetic and potential energy, is 


called the Lagrangian of the system, and Equation (6.5 1 is called the Euler 


Lagrange Equation. The Euler-Lagrange equations provide a formulation of 
the dynamic equations of motion equivalent to those derived using Newton’s 
Second Law. However, as we shall see, the Lagrangian approach is advantageous 
for more complex systems such as multi-link robots. 

Example: 6.1 Single-Link Manipulator 

Consider the single-link robot arm shown in Figure [672] consisting of a rigid 


Link 



Fig. 6.2 Single-Link Robot. 

link coupled through a gear train to a DC-motor. Let 6t and 6 m denote the 
angles of the link and motor shaft, respectively. Then 9 m = r9e where r : 1 is 
the gear ratio. The algebraic relation between the link and motor shaft angles 
means that the system has only one degree-of-freedom and we can therefore 
write the equations of motion using either 9 m or 9(. In terms of 9p : the kinetic 
energy of the system is given by 

K = 2 + 2 

= ^ { r2 Jm + Jg)&£ 


( 6 . 6 ) 
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where J m , Jf, are the rotational inertias of the motor and link, respectively. The 
potential energy is given as 


P = M g£(l — cos Oe) (6.7) 

where M is the total mass of the link and £ is the distance from the joint axis 
to the link center of mass. Defining J = r 2 J m + Je, the Lagrangian £ is given 

by 


£ = — Mg£(l — cos 9e) (6.8) 

Substituting this expression into the Euler-Lagrange equations yields the equa- 
tion of motion 


J6e + Mg£sm6e = T£ (6.9) 

The generalized force r £ represents those external forces and torques that 
are not derivable from a potential function. For this example, T£ consists of the 
motor torque u = rr m , reflected to the link, and (nonconservative) damping 
torques B m 6 m , and B£,0£. Reflecting the motor damping to the link yields 

r = u — BO £ 

where B — rB m + B(. Therefore the complete expression for the dynamics of 
this system is 


J 0 £ T BO £ T M gl sin 0£ = u 


( 6 . 10 ) 


In general, for any system of the type considered, an application of the Euler- 
Lagrange equations leads to a system of n coupled, second order nonlinear 
ordinary differential equations of the form 


Euler-Lagrange Equations 


d_ d£ __ d£ 

dt dqi dqi 


n i = 


( 6 . 11 ) 


The order, n, of the system is determined by the number of so-called gener- 
alized coordinates that are required to describe the evolution of the system. 
We shall see that the n Denavit-Hartenberg joint variables serve as a set of 
generalized coordinates for an n-link rigid robot. 


6.1.2 The General Case 

Now, consider a system of k particles, with corresponding position vectors 
r i, . . . ,rfc. If these particles are free to move about without any restrictions, 
then it is quite an easy matter to describe their motion, by noting that the 
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Fig. 6.3 System of k particles 


rate of change of the momentum of each mass equals the external force applied 
to it. However, if the motion of the particles is constrained in some fashion, 
then one must take into account not only the externally applied forces, but 
also the so-called constraint forces, that is, the forces needed to make the 
constraints hold. As a simple illustration of this, suppose the system consists 
of two particles, which are joined by a massless rigid wire of length l. Then the 
two coordinates rq and r 2 must satisfy the constraint 

\\ri-r 2 \\=e, or (rq - r 2 ) T (rq - r 2 ) = l 2 (6.12) 

If one applies some external forces to each particle, then the particles experience 
not only these external forces but also the force exerted by the wire, which is 
along the direction r 2 — rq and of appropriate magnitude. Therefore, in order 
to analyze the motion of the two particles, we can follow one of two options. 
We can compute, under each set of external forces, what the corresponding 
constraint force must be in order that the equation above continues to hold. 
Alternatively, we can search for a method of analysis that does not require us 
to know the constraint force. Clearly, the second alternative is preferable, since 
it is in general quite an involved task to compute the constraint forces. The 
contents of this section are aimed at achieving this latter objective. 

First it is necessary to introduce some terminology. A constraint on the k 
coordinates rq, . . . , rq is called holonomic if it is an equality constraint of the 
form 


9i(ri, ...,r k ) = 0, 


i = l,. . 


(6.13) 


and nonholonomic otherwise. The constraint (6.12) imposed by connecting 


two particles by a massless rigid wire is a holonomic constraint. As as example of 
a nonholonomic constraint, consider a particle moving inside a sphere of radius 
p centered at the origin of the coordinate system. In this case the coordinate 
vector r of the particle must satisfy the constraint 


\H < P 


(6.14) 
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Note that the motion of the particle is unconstrained so long as the particle 
remains away from the wall of the sphere; but when the particle comes into 
contact with the wall, it experiences a constraining force. 

If a system is subjected to t holonomic constraints, then one can think in 
terms of the constrained system having l fewer degrees-of-freedom than the 
unconstrained system. In this case it may be possible to express the coordinates 
of the k particles in terms of n generalized coordinates q \ .... . q n . In other 
words, we assume that the coordinates of the various particles, subjected to the 


set of constraints (6.131, can be expressed in the form 


fi = r i (q 1 ,...,q n ), i=l,...,k 


(6.15) 


where q±, ... ,q n are all independent. In fact, the idea of generalized coordinates 
can be used even when there are infinitely many particles. For example, a 
physical rigid object such as a bar contains an infinity of particles; but since 
the distance between each pair of particles is fixed throughout the motion of the 
bar, only six coordinates are sufficient to specify completely the coordinates of 
any particle in the bar. In particular, one could use three position coordinates 
to specify the location of the center of mass of the bar, and three Euler angles 
to specify the orientation of the body. To keep the discussion simple, however, 
we assume in what follows that the number of particles is finite. Typically, 
generalized coordinates are positions, angles, etc. In fact, in Chapter [3] we 
chose to denote the joint variables by the symbols q±, . . . ,q n precisely because 
these joint variables form a set of generalized coordinates for an n-link robot 
manipulator. 

One can now speak of virtual displacements, which are any set, Sri , . . . , Srk, 
of infinitesimal displacements that are consistent with the constraints. For ex- 


ample, consider once again the constraint (6.12) and suppose ri,r 2 are per- 
turbed to ri + Sri, r 2 + Sr 2 , respectively. Then, in order that the perturbed 
coordinates continue to satisfy the constraint, we must have 


(ri + Sri — r 2 — Sr 2 ) T (ri + Sr i — r 2 — Sr 2 ) = £ 2 


(6.16) 


Now let us expand the above product and take advantage of the fact that 
the original coordinates ri,r 2 satisfy the constraint (6.121; let us also neglect 
quadratic terms in Sr\,Sr 2 . This shows that 


(ri - r 2 ) T (5ri - Sr 2 ) = 0 


(6.17) 


Thus any perturbations in the positions of the two particles must satisfy the 
above equation in order that the perturbed positions continue to satisfy the 


constraint (6.12). Any pair of infinitesimal vectors Sri,Sr 2 that satisfy (6.171 


would constitute a set of virtual displacements for this problem. 

Now the reason for using generalized coordinates is to avoid dealing with 
complicated relationships such as (6.171 above. If (6.151 holds, then one can 
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see that the set of all virtual displacements is precisely 

n r. 

Sri = '^Z~p L Sqj, i = 1, . . . ,k (6.18) 

i=i ° qj 

where the virtual displacements 5q \, . . . , Sq n of the generalized coordinates are 
unconstrained (that is what makes them generalized coordinates). 

Next we begin a discussion of constrained systems in equilibrium. Suppose 
each particle is in equilibrium. Then the net force on each particle is zero, 
which in turn implies that the work done by each set of virtual displacements 
is zero. Hence the sum of the work done by any set of virtual displacements is 
also zero; that is, 


k 

J2 F i 6r i = 0 ( 6 - 19 ) 

i= 1 

where Fi is the total force on particle i. As mentioned earlier, the force Fi 
is the sum of two quantities, namely (i) the externally applied force f i , and 
(ii) the constraint force f\ a \ Now suppose that the total work done by the 
constraint forces corresponding to any set of virtual displacements is zero, that 
is, 


£(/} o) YSn = 0 


( 6 . 20 ) 


i = 1 


This will be true whenever the constraint force between a pair of particles is 
directed along the radial vector connecting the two particles (see the discussion 


in the next paragraph). Substituting (6.201 into (6.191 results in 


fi Sri 


= 0 


( 6 . 21 ) 


The beauty of this equation is that it does not involve the unknown constraint 
forces, but only the known external forces. This equation expresses the prin- 
ciple of virtual work, which can be stated in words as follows: 


Principle of Virtual Work 

The work done by external forces corresponding to any set of virtual displace- 
ments is zero. 


Note that the principle is not universally applicable, but requires that (6.201 
hold, that is, that the constraint forces do no work. Thus, if the principle of 
virtual work applies, then one can analyze the dynamics of a system without 
having to evaluate the constraint forces. 

It is easy to verify that the principle of virtual work applies whenever the 
constraint force between a pair of particles acts along the vector connecting the 
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position coordinates of the two particles. In particular, when the constraints 


are of the form (6.121, the principle applies. To see this, consider once again 


a single constraint of the form (6.121. In this case the constraint force, if any, 


must be exerted by the rigid massless wire, and therefore must be directed along 
the radial vector connecting the two particles. In other words, the force exerted 
on particle 1 by the wire must be of the form 


f[ a) = c(n - r 2 ) 


( 6 . 22 ) 


for some constant c (which could change as the particles move about). By the 
law of action and reaction, the force exerted on particle 2 by the wire must be 
just the negative of the above, that is, 


/(<*) 
J 2 


= — c(ri - r 2 ) 


(6.23) 


Now the work done by the constraint forces corresponding to a set of virtual 
displacements is 


(/i“ ) ) T 5r 1 + (fi a) ) T 6r 2 


= c(ri — r 2 ) T (5ri — Sr 2 ) 


(6.24) 


But (6.171 shows that for any set of virtual displacements, the above inner 


product must be zero. Thus the principle of virtual work applies in a system 


constrained by (6.12). The same reasoning can be applied if the system consists 


of several particles, which are pairwise connected by rigid massless wires of 
fixed lengths, in which case the system is subjected to several constraints of 
the form (6.12l. Now, the requirement that the motion of a body be rigid 


can be equivalently expressed as the requirement that the distance between 
any pair of points on the body remain constant as the body moves, that is, 
as an infinity of constraints of the form (6.12l. Thus the principle of virtual 


work applies whenever rigidity is the only constraint on the motion. There are 
indeed situations when this principle does not apply, typically in the presence 
of magnetic fields. However, in all situations encountered in this book, we can 
safely assume that the principle of virtual work is valid. 

In (6.211, the virtual displacements Sri are not independent, so we cannot 


conclude from this equation that each coefficient F, individually equals zero. In 
order to apply such reasoning, we must transform to generalized coordinates. 
Before doing this, we consider systems that are not necessarily in equilibrium. 
For such systems, D’Alembert’s principle states that, if one introduces a fic- 
titious additional force —pi on particle i for each i, where pi is the momentum 
of particle i, then each particle will be in equilibrium. Thus, if one modifies 


(6.19l by replacing Fj by Fi — Pi, then the resulting equation is valid for arbi- 


trary systems. One can then remove the constraint forces as before using the 
principle of virtual work. This results in the equations 


J2^ 6r i~J2Pi 5ri = 


i= 1 


i — 1 


0 


(6.25) 
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The above equation does not mean that each coefficient of Sri is zero. For this 
purpose, express each Sr.i in terms of the corresponding virtual displacements 
of generalized coordinates, as is done in (6.18 I . Then the virtual work done by 
the forces f , L is given by 


K K n rv n 

J2f? Sr i = E E = EE^ 

2 = 1 2=1 j — 1 j = 1 


where 


(6.26) 


V’i = J2f 


2=1 


'jn OTi 

d qj 


(6.27) 


is called the j-th generalized force. Note that 'ipj need not have dimensions 
of force, just as q j need not have dimensions of length; however, 4’j^Qj must 
always have dimensions of work. 

Now let us study the second summation in (6.251 Since pi = rriifi , it follows 
that 


k k k n 

E P r = E Sr * = EE m ^ 

*= i i= l 


,, r p dv j 

dqs 

2—1 2=1 ?.= 1 7 — 1 

Next, using the product rule of differentiation, we see that 
k k 


- V<!- 

m * r » T)n ~ 2^ I dt. 


2=1 


dq. 


2=1 


dt 


m i r i 


. ' ~p dv 2 


d( h 


■T 


dt 


s qj 


du 

d qj 


(6.28) 


(6.29) 


Now differentiate (6.15) using the chain rule; this gives 

n 0 

E dri . 

qj 

j= i d< ij 


Observe from the above equation that 

dvj 

den 

Next, 

’ dr i 


drj 

dqj 


dt 


dqj 


ZZ d 2 rj . _ duq 
f^d qj dq e qe dq, 


(6.30) 


(6.31) 


(6.32) 


where the last equality follows from (6.30). Substituting from (6.31 1 and (6.321 


into (6.29) and noting that ft = Vi gives 

k 


E 

2=1 


m i r i 


.. r J' 2 


dqj 


= E 


i= 1 


rriiVi 


t dvj 


d<ij 


- nriiVi 


rp dVi 


dqj 


(6.33) 
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If we define the kinetic energy K to be the quantity 

k „ 


K = ^-rriivfvi 


then the sum above can be compactly expressed as 

A ,. T dr i _ d dK dK 


(6.34) 


(6.35) 


(6.251 is 


Now, substituting from (6.351 into (6.28) shows that the second summation in 

(6.36) 


A , T . A r d 9A' dK] 


Finally, combining (6.361 and (6.261 gives 


E 

3=1 


f d dK 

dK 

\ dt dqj 

dqj 


A > hi = 0 


(6.37) 


Now, since the virtual displacements Sqj are independent, we can conclude that 
each coefficient in (6.371 is zero, that is, that 


d dK dK 
dt dqj dqj 


= A, j = l,...,n 


(6.38) 


If the generalized force ipj is the sum of an externally applied generalized force 
and another one due to a potential field, then a further modification is possible. 
Suppose there exist functions t 3 and a potential energy function P{q) such that 


Vb = - 


dP 

dqj 


+ Tj 


Then (6.381 can be written in the form 

dt dqj dqj 


(6.39) 


(6.40) 


where £ = K—P is the Lagrangian and we have recovered the Euler-Lagrange 
equations of motion as in Equation (6.111. 


6.2 GENERAL EXPRESSIONS FOR KINETIC AND POTENTIAL 
ENERGY 

In the previous section, we showed that the Euler-Lagrange equations can be 
used to derive the dynamical equations in a straightforward manner, provided 
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one is able to express the kinetic and potential energy of the system in terms of a 
set of generalized coordinates. In order for this result to be useful in a practical 
context, it is therefore important that one be able to compute these terms 
readily for an n-link robotic manipulator. In this section we derive formulas 
for the kinetic energy and potential energy of a rigid robot using the Denavit- 
Hartenberg joint variables as generalized coordinates. 

To begin we note that the kinetic energy of a rigid object is the sum of two 
terms: the translational energy obtained by concentrating the entire mass of 
the object at the center of mass, and the rotational kinetic energy of the body 
about the center of mass. Referring to Figure [R4| we attach a coordinate frame 
at the center of mass (called the body attached frame ) as shown. The kinetic 



Fig. 6.4 A General Rigid Body 

energy of the rigid body is then given as 

/C = ^mv T v + ^u T lu) (6-41) 

where m is the total mass of the object, v and u are the linear and angular 
velocity vectors, respectively, and X is a symmetric 3x3 matrix called the 

Inertia Tensor. 

6.2.1 The Inertia Tensor 

It is understood that the linear and angular velocity vectors, v and u>, respec- 
tively, in the above expression for the kinetic energy are expressed in the inertial 
frame. In this case we know that w is found from the skew symmetric matrix 

S{uj) = RR t (6.42) 

where R is the orientation transformation between the body attached frame and 
the inertial frame. It is therefore necessary to express the inertia tensor, X, also 
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in the inertial frame in order to compute the triple product io T Iu). The inertia 
tensor relative to the inertial reference frame will depend on the configuration 
of the object. If we denote as I the inertia tensor expressed instead in the body 
attached frame, then the two matrices are related via a similarity transformation 
according to 


I = RIR t 


(6.43) 


This is an important observation because the inertia matrix expressed in the 
body attached frame is a constant matrix independent of the motion of the ob- 
ject and easily computed. We next show how to compute this matrix explicitly. 

Let the mass density of the object be represented as a function of position, 
p{ x, y , z ). Then the inertia tensor in the body attached frame is computed as 


where 


I 


Ixx Cry * x z 

lyx lyy lyz 

d-zx d zy 1 zz 


d-xy ^yx 


lyz — Izy 



2 + z 2 )p(x, y, z)dx dy dz 
2 + z 2 )p(x, y , z)dx dy dz 
2 + y 2 )p{x,y,z)dx dy dz 
xyp(x,y, z)dx dy dz 
xzp(x,y, z)dx dy dz 
yzp(x, y, z)dx dy dz 


(6.44) 


The integrals in the above expression are computed over the region of space 
occupied by the rigid body. The diagonal elements of the inertia tensor, I xx , 
lyy, Izz, are called the Principal Moments of Inertia about the x,y,z axes, 
respectively. The off diagonal terms I xy , I xz , etc., are called the Cross Prod- 
ucts of Inertia. If the mass distribution of the body is symmetric with respect 
to the body attached frame then the cross products of inertia are identically 
zero. 


Example: 6.2 Uniform Rectangular Solid 

Consider the rectangular solid of length, a, width, b , and height, c, shown in 
Figure 6.5 and suppose that the density is constant, p(x,y,z) = p. 

If the body frame is attached at the geometric center of the object, then by 
symmetry, the cross products of inertia are all zero and it is a simple exercise 
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Fig. 6.5 Uniform Rectangular Solid 


to compute 
Ixx — 
Likewise 


/■c/2 pb / 2 pa/2 , 

/ / / (y + Z 2 )p(x,y,z)dx dy dz = p— (& 2 + c 2 ) 

J-c/2 J-b/2 J —a/2 


abc . o 2\ t Clbc , <2 7 2\ 

lyy = C ’ I zz ^^2 ° ^ ) 


and the cross products of inertia are zero. 


6.2.2 Kinetic Energy for an n-Link Robot 

Now consider a manipulator consisting of n links. We have seen in Chapter [4] 
that the linear and angular velocities of any point on any link can be expressed 
in terms of the Jacobian matrix and the derivative of the joint variables. Since 
in our case the joint variables are indeed the generalized coordinates, it follows 
that, for appropriate Jacobian matrices J Vi and J UJi , we have that 

Vi = J Vi (q)q , Ui = J Ui {q)q (6.45) 


Now suppose the mass of link i is to, and that the inertia matrix of link i, 
evaluated around a coordinate frame parallel to frame i but whose origin is at 
the center of mass, equals Then from (6.41 1 it follows that the overall kinetic 
energy of the manipulator equals 

1 n 

K = - q T '^2,[rn i J Vi {q) T J Vi (q) + J Ui (q) T Ri(q)IiRi(q) T J Ui {q)\q (6.46) 

2=1 


In other words, the kinetic energy of the manipulator is of the form 

K = l ~q T D{q)q 


(6.47) 


200 DYNAMICS 


where D{q) is a symmetric positive definite matrix that is in general configura- 
tion dependent. The matrix D is called the inertia matrix, and in Section |6~i| 
we will compute this matrix for several commonly occurring manipulator con- 
figurations. 


6.2.3 Potential Energy for an n-Link Robot 

Now consider the potential energy term. In the case of rigid dynamics, the only 
source of potential energy is gravity. The potential energy of the i-th link can 
be computed by assuming that the mass of the entire object is concentrated at 
its center of mass and is given by 

Pi = g T r C imi (6.48) 

where g is vector giving the direction of gravity in the inertial frame and the 
vector r C i gives the coordinates of the center of mass of link i. The total potential 
energy of the n-link robot is therefore 

n n 

p = y, p, = g T r ci mi (6.49) 

i— 1 2—1 

In the case that the robot contains elasticity, for example, flexible joints, then 
the potential energy will include terms containing the energy stored in the 
elastic elements. 

Note that the potential energy is a function only of the generalized coor- 
dinates and not their derivatives, i.e. the potential energy depends on the 
configuration of the robot but not on its velocity. 


6.3 EQUATIONS OF MOTION 

In this section, we specialize the Euler-Lagrange equations derived in Section [6.1| 
to the special case when two conditions hold: first, the kinetic energy is a 
quadratic function of the vector q of the form 

1 n 

K = -Y^ d ij(q)QiQj := D(q)q (6.50) 

ij 

where the n x n “inertia matrix” D{q) is symmetric and positive definite for 
each q £ R n , and second, the potential energy P = P(q) is independent of q. 
We have already remarked that robotic manipulators satisfy this condition. 

The Euler-Lagrange equations for such a system can be derived as follows. 
Since 

= K-P= ~P{q) 

i,3 


L 


(6.51) 
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we have that 


dL 

dqk 


— dkj q 3 


(6.52) 


and 


d dL .. d 

dtWk = 2^ dk M + 2^dt dkjqj 

i 3 

E dk ^o + E 


i,3 


dqi 


Also 


dL 

dqk 


1 ddij 


dP 

2 ^ dqk ^ dqk 


Thus the Euler-Lagrange equations can be written 


E dfc ^ + J 2 




dd, 


kj 


1 ddi 


dqi 2 dq k 


qidj 


dP 

dq k 


= T k 


(6.53) 


(6.54) 


(6.55) 


By interchanging the order of summation and taking advantage of symmetry, 
we can show that 


E 


ddkj 

dqi 


QiQj 


1 \ v f ddkj ddki 1 . 


= E 


dqi dq 3 J 


Mj 


Hence 


E 


dd 


kj 


1 ddi 


dqi 2 dq k 


m = E 


1 (dd 


*,3 


ddki ddij 
dqi ' dq 3 dq k 


kj 


Christoffel Symbols of the First Kind 


C-ij k • — 


1 f dd k n , ddki ddi 


dq t dq 3 dq k 


(6.56) 


QiQj (6.57) 


(6.58) 


The terms Ci 3k in Equation (6.58) are known as Christoffel symbols (of the 
first kind). Note that, for a fixed k , we have Cij k = c 3 i k , which reduces the effort 
involved in computing these symbols by a factor of about one half. Finally, if 
we define 


dP 

Ok = -X— 

dqk 

then we can write the Euler-Lagrange equations as 

E + E ^jki^QiQj + <t>k(q ) = n, 


(6.59) 


k = 1, . . . , n (6.60) 
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In the above equation, there are three types of terms. The first involve 
the second derivative of the generalized coordinates. The second are quadratic 
terms in the first derivatives of q , where the coefficients may depend on q. These 
are further classified into two types. Terms involving a product of the type qf 
are called centrifugal, while those involving a product of the type gyp where 
* / j are called Coriolis terms. The third type of terms are those involving 
only q but not its derivatives. Clearly the latter arise from differentiating the 


potential energy. It is common to write (6.60 1 in matrix form as 


Matrix Form of Euler-Lagrange Equations 

D(q)q + C{q,q)q + g(q) = r 
where the k,j - th element of the matrix C(q,q) is defined as 

n 

Ckj ^ ] cjjk(q)qi 


(6.61) 


(6.62) 


- £5 


1 ( ddkj ddki ddi 


2 ( dqj dqj 


dqk 


Qi 


Let us examine an important special case, where the inertia matrix is diag- 


onal and independent of q. In this case it follows from (6.58) that all of the 
Christoffel symbols are zero, since each dij is a constant. Moreover, the quan- 


tity dkj is nonzero if and only if k = j , so that the Equations 6.60 1 decouple 
nicely into the form 


dkkq- 4>k(q) =r k , k = l,...,n (6.63) 

In summary, the development in this section is very general and applies to 
any mechanical system whose kinetic energy is of the form (6.50 1 and whose 
potential energy is independent of q. In the next section we apply this discussion 
to study specific robot configurations. 


6.4 SOME COMMON CONFIGURATIONS 

In this section we apply the above method of analysis to several manipulator 
configurations and derive the corresponding equations of motion. The config- 
urations are progressively more complex, beginning with a two-link cartesian 
manipulator and ending with a five-bar linkage mechanism that has a particu- 
larly simple inertia matrix. 


Two-Link Cartesian Manipulator 


Consider the manipulator shown in Figure 6.6 consisting of two links and two 


SOME COMMON CONFIGURATIONS 


203 



Fig. 6.6 Two-link cartesian robot. 


prismatic joints. Denote the masses of the two links by mi and m 2 , respec- 
tively, and denote the displacement of the two prismatic joints by q\ and ( 72 , 
respectively. Then it is easy to see, as mentioned in Section [6T] that these two 
quantities serve as generalized coordinates for the manipulator. Since the gen- 
eralized coordinates have dimensions of distance, the corresponding generalized 
forces have units of force. In fact, they are just the forces applied at each joint. 
Let us denote these by fi, i = 1, 2. 

Since we are using the joint variables as the generalized coordinates, we know 
that the kinetic energy is of the form (6.50) and that the potential energy is 


only a function of q\ and < 72 - Hence we can use the formulae in Section |6.3| 
to obtain the dynamical equations. Also, since both joints are prismatic, the 
angular velocity Jacobian is zero and the kinetic energy of each link consists 
solely of the translational term. 

It follows that the velocity of the center of mass of link 1 is given by 


where 


Similarly, 


Vd Jv c iQ 


J 1! 


0 0 
0 0 
1 0 



v C 2 = Jv c2 q 


where 


J u 


0 0 
0 1 
1 0 


(6.64) 


(6.65) 


( 6 . 66 ) 


(6.67) 
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Hence the kinetic energy is given by 


K = -q T {m 1 Jv c J Vcl +m 2 Jv c2 J Vc2 } q 


( 6 . 68 ) 


Comparing with (6.50), we see that the inertia matrix D is given simply by 


D 


m\ + m 2 0 
0 m 2 


(6.69) 


Next, the potential energy of link 1 is m\gqi , while that of link 2 is m 2 gqi , 
where g is the acceleration due to gravity. Hence the overall potential energy is 


P = g{m\ + m 2 )q\ 


(6.70) 


Now we are ready to write down the equations of motion. Since the inertia 
matrix is constant, all Christoffel symbols are zero. Further, the vectors <j>k are 
given by 


h = = g{m i 


dqi 


m 2 ), 


, dP n 
q>2 = w— = o 
oq2 


Substituting into (6.601 gives the dynamical equations as 

(mi + m 2 )qi + g(mi + m 2 ) = /i 

m 2 q 2 = f 2 


(6.71) 


(6.72) 


Planar Elbow Manipulator 


Now consider the planar manipulator with two revolute joints shown in Fig- 
ure [G77] Let us fix notation as follows: For i = 1,2, ^ denotes the joint angle, 
which also serves as a generalized coordinate; rrii denotes the mass of link i, £i 
denotes the length of link i\ i C i denotes the distance from the previous joint to 
the center of mass of link i\ and Ii denotes the moment of inertia of link i about 
an axis coming out of the page, passing through the center of mass of link i. 

We will make effective use of the Jacobian expressions in Chapter [4] in com- 
puting the kinetic energy. Since we are using joint variables as the generalized 
coordinates, it follows that we can use the contents of Section |6.7| First, 


Vd — Jv c iQ 


where, 


J i< 


—l c smqi 0 
4i cos q± 0 
0 0 


(6.73) 


(6.74) 


ii c2 — 


Similarly, 


( 6 . 75 ) 


SOME COMMON CONFIGURATIONS 205 



Fig. 6.7 Two-link revolute joint arm. 


where 


J u 


-l\ sin qx - l c2 sin (qx + q 2 ) 
lx cos qx + l c 2 cos(gi + q 2 ) 
0 


-l c2 sm(qx + q 2 ) 
lc 2 cos(gi + q 2 ) 
0 


(6.76) 


Hence the translational part of the kinetic energy is 


^mxv^xVd + 7^rn 2 Vc 2 v c2 


-g{miJj cl J Vcl + m. 2 Jy c2 J Vo2 } q 


(6.77) 


Next we deal with the angular velocity terms. Because of the particularly 
simple nature of this manipulator, many of the potential difficulties do not arise. 
First, it is clear that 


u>x = qxk , lu 2 = (qx + q 2 )k 


(6.78) 


when expressed in the base inertial frame. Moreover, since u>i is aligned with 
k, the triple product ujli.uii reduces simply to (I^i times the square of the 
magnitude of the angular velocity. This quantity ( 133 ),: is indeed what we have 
labeled as li above. Hence the rotational kinetic energy of the overall system is 



1 0 
0 0 


+ 12 



(6.79) 


Now we are ready to form the inertia matrix D(q). For this purpose, we 
merely have to add the two matrices in (6.771 and (|6.79 1, respectively. Thus 


h + 1 2 I 2 
I2 I2 


D(q) — m xJ Vcl Jv cl + m 2 J Vc2 J Vc2 


(6.80) 
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Carrying out the above multiplications and using the standard trigonometric 
identities cos 2 9 + sin 2 9 = 1, cos a cos (3 + sin a sin f3 = cos (a — /?) leads to 

dn = mi^i + m 2 ((^ H~ Z\ 2 4” 2^it? 2 2 + 2£i£ c2 cos q 2 ) + I\ + I 2 
di 2 = d 21 = n r l 2 (£ c 2 + 442 COS <72) + 4 

e?22 = 2 + 4 (6.81) 


Now we can 
gives 


compute the Christoffel symbols using the definition (6.58|). This 


cm 

C121 

C221 

C112 

Cl22 

C222 


1 ddn 

2 dqi 

C211 = 

ddi2 _ 
dq 2 
dd 2 i _ 
dqi 

C212 = 


= 0 

1 ddn 

2 dq 2 

1 dd 22 

2 dqi 

1 ddn 

2 dq 2 

1 dd 22 

2 dgi 


= — m 2 ^i^c2 sing 2 =: h 


= h 


= -h 


= 0 


1 dd 22 

2 dq 2 


(6.82) 


Next, the potential energy of the manipulator is just the sum of those of the 
two links. For each link, the potential energy is just its mass multiplied by the 
gravitational acceleration and the height of its center of mass. Thus 


Pi = migZci sin q 1 

P 2 = m 2 g{Zi sin qi + Z c2 sin(gi + q 2 )) 

P = Pi + P 2 = (mi4i + m 2 Zi)g sin q x + m 2 Z c2 g sin(gi + q 2 ) (6.83) 


Hence, the functions 4>k defined in (6.591 become 


02 


= (mi4i + m 2 £i)gcosqi + m 2 Z c2 g cos(gi + q 2 ) 
dqi 


dP 

dq 2 


= m 2 i c2 cos(gi + q 2 ) 


(6.84) 

(6.85) 


Finally we can write down the dynamical equations of the system as in 
Substituting for the various quantities in this equation and omitting zero 
leads to 


( f6T60] >. 

terms 


du'qi + di 2 q 2 + Ci 2 iqiq 2 + c 2 nq 2 qi + 0221^2 + 01 

d 2 iqi + d 22 q 2 + Cn2<7i + 02 

In this case the matrix C(q,q) is given as 


C = 


hq 2 hq 2 + hq x 
—hqi 0 


Tl 

t 2 ( 6 . 86 ) 


(6.87) 
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Planar Elbow Manipulator with Remotely Driven Link 

Now we illustrate the use of Lagrangian equations in a situation where the 
generalized coordinates are not the joint variables defined in earlier chapters. 
Consider again the planar elbow manipulator, but suppose now that both joints 
are driven by motors mounted at the base. The first joint is turned directly by 
one of the motors, while the other is turned via a gearing mechanism or a timing 
belt (see Figure |fT8]). In this case one should choose the generalized coordinates 



Fig. 6.8 Two-link revolute joint arm with remotely driven link, 
as shown in Figure |6.9| because the angle P 2 is determined by driving motor 



Fig. 6.9 Generalized coordinates for robot of Figure 6.4. 

number 2, and is not affected by the angle p\. We will derive the dynamical 
equations for this configuration, and show that some simplifications will result. 

Since p\ and P 2 are not the joint angles used earlier, we cannot use the 
velocity Jacobians derived in Chapter [4] in order to find the kinetic energy of 
each link. Instead, we have to carry out the analysis directly. It is easy to see 
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that 


-l c i sinp! 


ti sinpj 

—i C 2 sin p 2 


It] 

id cos pi 

Pi, Vc2 = 

ii cos pi 

4 2 cosp 2 


0 

0 

0 


. P 1 2 


Vd = 


u>i = pik, u>2 = i>ik 
Hence the kinetic energy of the manipulator equals 

1 


K 


= D (p)p 


where 


D(p) = 


Computing the Christoffel symbols as in (6.58) gives 

1 ddn 


cm — 


C121 = 


C221 = 


C112 = 


C212 — 


C222 — 


2 dpi 

1 ddii 

C211 = v — — = 0 


dd 


12 


dp 2 

dd 2 i 

dpi 


2 dp 2 

1 dd 2 2 

2 dpi 

1 ddn 

2 dp 2 


= —m 2 li(-c2 sin(p2 ~Pi) 

= m 2 h£c 2 sin(p 2 ~ Pi) 


1 dd 2 2 „ 

— C122 — 0 „ — — 0 

2 dpi 

1 dd 22 _ 

2 dp 2 ~ 


mil 2 cl + m 2 i\ + h m 2 £i( C 2 cos (p 2 - Pi) 

m 2 h£c2 cos(p 2 - Pi) m 2 i 2 2 + ^2 


( 6 . 88 ) 


(6.89) 


(6.90) 


(6.91) 


(6.92) 


Next, the potential energy of the manipulator, in terms of pi and p 2 , 
P = migtd sinpi + m 2 g(£i sinpi + i c2 sinp 2 ) 
Hence the gravitational generalized forces are 

(f) 1 = (mi£ c i + m 2 £i)gcospi 

4> 2 = m 2 (- C 2 gcosp 2 

Finally, the equations of motion are 

dnPi + d\ 2 p 2 + C221P2 + 4 >i = r i 

d 2 lPl + d 2 2P2 + Cii2p\ + & = t 2 


equals 

(6.93) 


Comparing (6.941 and (6.86), we see that by driving the second joint 
from the base we have eliminated the Coriolis forces, but we still 
centrifugal forces coupling the two joints. 


(6.94) 

remotely 
have the 
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Fig. 6.10 Five-bar linkage 


Five-Bar Linkage 

Now consider the manipulator shown in Figure |6.10| We will show that, if the 
parameters of the manipulator satisfy a simple relationship, then the equations 
of the manipulator are decoupled, so that each quantity <71 and (72 can be con- 
trolled independently of the other. The mechanism in Figure |G.10| is called a 
five-bar linkage. Clearly there are only four bars in the figure, but in the 
theory of mechanisms it is a convention to count the ground as an additional 
linkage, which explains the terminology. In Figure |6.10| it is assumed that the 
lengths of links and 3 are the same, and that the two lengths marked t 2 are the 
same; in this way the closed path in the figure is in fact a parallelogram, which 
greatly simplifies the computations. Notice, however, that the quantities £ c t , 
and £ C 3 need not be equal. For example, even though links 1 and 3 have the 
same length, they need not have the same mass distribution. 

It is clear from the figure that, even though there are four links being moved, 
there are in fact only two degrees-of- freedom, identified as q± and qi- Thus, in 
contrast to the earlier mechanisms studied in this book, this one is a closed 
kinematic chain (though of a particularly simple kind). As a result, we cannot 
use the earlier results on Jacobian matrices, and instead have to start from 
scratch. As a first step we write down the coordinates of the centers of mass of 
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the various links as a function of the generalized coordinates. This gives 


Xc\ 


4 i COS qi 



Vcl 


4 i sin qi 



X C 2 


4 2 cos q 2 



Vc2 


42 sin <72 



X C 3 

Vc3 

= 

i c2 cos qi 
42 sin <72 

+ 

4 3 cos 91 

4 3 Sill (/! 

X c 4 

Vc4 . 

= 

4 cos q\ 

4 sin <7! 

+ 

44 cos (32 
4 4 sin (32 



£1 cos <71 


44 cos <72 



£1 sin <71 


4-1 sin 32 


(6.95) 

(6.96) 

(6.97) 


(6.98) 


Next, with the aid of these expressions, we can write down the velocities of 
the various centers of mass as a function of q± and <72- For convenience we drop 
the third row of each of the following Jacobian matrices as it is always zero. 
The result is 


«cl = 

— 4i sin (71 0 

4i cos gi 0 

(1 

V C 2 = 

0 —4 2 sin 32 

0 4 2 cos 32 

(1 

V C 3 = 

—4 3 sin (71 -4 sin g 2 
£ c3 cos qi £ 2 cos 32 

Vc4 = 

— £1 sin 31 4 4 sin 32 
£1 cos 31 £ c4 cos 32 


Let us define the velocity Jacobians J Vci , i £ {1, ... ,4} in the obvious fashion, 
that is, as the four matrices appearing in the above equations. Next, it is clear 
that the angular velocities of the four links are simply given by 

u>i = 0J3 = qik, u>2 = W4 = q2k. (6.100) 


Thus the inertia matrix is given by 


D{q) 


4 

^ ^ TfliJ VC J vc T 

i — 1 


h + h 0 

0 I‘2 + I4 


( 6 . 101 ) 


If we now substitute from (6.991 into the above equation and use the standard 
trigonometric identities, when the dust settles we are left with 


dn(q) 

d\2{q) 

d22{q) 


m l^cl T m 3^c3 + TO 4 £ 3 + I\ + I3 
d 2 i{q) = (m 3 £ 2 £ c 3 - mditci) cos (q 2 - qi) (6.102) 

2 + 2 + rnd 2 cA + I2 + h 
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Now we note from the above expressions that if 


= md\(-cA (6.103) 

then g?i 2 and c? 2 i are zero, i.e. the inertia matrix is diagonal and constant. 
As a consequence the dynamical equations will contain neither Coriolis nor 
centrifugal terms. 

Turning now to the potential energy, we have that 

4 

p = 9^2 Vd 

i= 1 

= gsmqi(mi£ c i + m 3£ C 3 + m^i) (6.104) 

+ g sin q 2 (rri 2 lc 2 + rn 3 i 2 — rn^l^) 

Hence 

4> i = 0cosgi(mi4i + m 3 ^c3 + mdi) 

4> 2 = g cos q 2 {rri 2 £c 2 + m 3 £2 — m^ci) (6.105) 


Notice that <j> \ depends only on q\ but not on <72 and similarly that (f > 2 depends 
only on q 2 but not on q lm Hence, if the relationship (6.103) is satisfied, then the 
rather complex-looking manipulator in Figure [6T0| is described by the decoupled 
set of equations 


duqi + = n, d 22 q 2 + $ 2 ( 92 ) = t 2 


(6.106) 


This discussion helps to explain the popularity of the parallelogram configu- 
ration in industrial robots. If the relationship (6.1031 is satisfied, then one can 
adjust the two angles <71 and q 2 independently, without worrying about inter- 
actions between the two angles. Compare this with the situation in the case of 
the planar elbow manipulators discussed earlier in this section. 


6.5 PROPERTIES OF ROBOT DYNAMIC EQUATIONS 

The equations of motion for an n-link robot can be quite formidable especially 
if the robot contains one or more revolute joints. Fortunately, these equations 
contain some important structural properties which can be exploited to good 
advantage in particular for developing control algorithms. We will see this 
in subsequent chapters. Here we will discuss some of these properties, the 
most important of which are the so-called skew symmetry property and the 
related passivity property, and the linearity in the parameters property. 
For revolute joint robots, the inertia matrix also satisfies global bounds that 
are useful for control design. 
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6.5.1 The Skew Symmetry and Passivity Properties 

The Skew Symmetry property refers to an important relationship between 
the inertia matrix D{q ) and the matrix C(q , q) appearing in (6.61 ) that will be 
of fundamental importance for the problem of manipulator control considered 
in later chapters. 

Proposition: 6.1 The Skew Symmetry Property 

Let D{q) be the inertia matrix for an n-link robot and define C(q, q) in terms of 
the elements of D(q) according to Equation (6.62). Then the matrix N(q,q) = 
D(q) — 2 C(q,q) is skew symmetric , that is, the components rijk of N satisfy 

Tljk = Tlfcj . 

Proof: Given the inertia matrix D(q), the kj - th component of D(q) is given 
by the chain rule as 




v-' ddkj 

= E - * 

i=l 


dqi 


Therefore, the kj-ih component of N = D — 2C is given by 


'R'kj — 


dkj ^Ckj 
ddkj 
dq t 


(6.107) 


(6.108) 


E 

2=1 L 
n 

E 

2=1 


ddkj ddki ddi 


dq, 

ddki 
dqk dqj 


d, 


dq k 


Qi 


dda 




Since the inertia matrix D(q) is symmetric, that is, dij = dji, it follows from 
(6.1081 by interchanging the indices k and j that 


W'jk — 


'H'kj 


(6.109) 


which completes the proof. 

It is important to note that, in order for N = D — 2 C to be skew-symmetric, 
one must define C according to Equation (6.621. This will be important in later 
chapters when we discuss robust and adaptive control algorithms. 

Related to the skew symmetry property is the so-called Passivity Property 
which, in the present context, means that there exists a constant, f3 > 0, such 
that 

The Passivity Property 

/ V(CMCR > -/3, v t > o 

Jo 


( 6 . 110 ) 


The term q T r has units of power and therefore the expression J 0 T q T (()r(<()d( 
is the energy produced by the system over the time interval [0,T]. Passivity 
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therefore means that the amount of energy dissipated by the system has a 
lower bound given by — /?. The word passivity comes from circuit theory where 
a passive system according to the above definition is one that can be built 
from passive components (resistors, capacitors, inductors). Likewise a passive 
mechanical system can be built from masses, springs, and dampers. 

To prove the passivity property, let H be the total energy of the system, i.e., 
the sum of the kinetic and potential energies, 


H= l -q T D{q)q + P{q) 


( 6 . 111 ) 


Then, the derivative H satisfies 


H = q T D{q)q+ l -q T b{q)q + q T ^ 

1 . 

= q T {T~C( qi q)~g(q)}+-q T D( q )q + q T — 


( 6 . 112 ) 


where we have substituted for D(q)q using the equations of motion. Collecting 

r) P 

terms and using the fact that g(q) = % yields 


1 


H = q 1 r+-q 1 {D(q)-2C(q,q)}q 


(6.113) 


•T 

= q t 


the latter equality following from the skew-symmetry property. Integrating 
both sides of (6.1131 with respect to time gives, 

[ <f(CMCR = H(T) - H{ 0) > -H( 0) (6.114) 

Jo 

since the total energy H(T ) is non-negative, and the passivity property there- 
fore follows with /3 = H( 0). 


6.5.2 Bounds on the Inertia Matrix 

We have remarked previously that the inertia matrix for an n-link rigid robot is 
symmetric and positive definite. For a fixed value of the generalized coordinate 
q , let 0 < Ai (q) < ...,< \ n (q) denote the n eigenvalues of D(q). These 
eigenvalues are positive as a consequence of the positive definiteness of D{q). 
As a result, it can easily be shown that 

Bounds on the Inertia Matrix 

Ai {q)Inxn < D(q) < A n(q)lnxn (6.115) 

where I nX n denotes the n x n identity matrix. The above inequalities are 
interpreted in the standard sense of matrix inequalities, namely, if A and B are 
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n x n matrices, then B < A means that the matrix A — B is positive definite 
and B < A means that A — B is positive semi-definite. 

If all of the joints are revolute then the inertia matrix contains only bounded 
functions of the joint variables, i.e., terms containing sine and cosine functions. 
As a result one can find constants A m and Am that provide uniform (indepen- 
dent of q ) bounds in the inertia matrix 

^m-^nxn — D{q) < A M Inxn < OO (6.116) 

6.5.3 Linearity in the Parameters 

The robot equations of motion are defined in terms of certain parameters, such 
as link masses, moments of inertia, etc., that must be determined for each 
particular robot in order, for example, to simulate the equations or to tune 
controllers. The complexity of the dynamic equations makes the determination 
of these parameters a difficult task. Fortunately, the equations of motion are 
linear in these inertia parameters in the following sense. There exists annxl 
function, Y ( q , q, q ), which we assume is completely known, and an ^-dimensional 
vector 0 such that the Euler-Lagrange equations can be written 

The Linearity in the Parameters Property 

D(q) + C(q,q)q + g(q) =: Y(q,q,q)Q = r (6.117) 

The function, Y(q,q,q), is called the Regressor and 0 is the Parameter 
vector. The dimension of the parameter space, i.e., the number of param- 
eters needed to write the dynamics in this way, is not unique. In general, a 
given rigid body is described by ten parameters, namely, the total mass, the 
six independent entries of the inertia tensor, and the three coordinates of the 
center of mass. An n-link robot then has a maximum of lOn dynamics pa- 
rameters. However, since the link motions are constrained and coupled by the 
joint interconnections, there are actually fewer than lOn independent parame- 
ters. Finding a minimal set of parameters that can parametrize the dynamic 
equations is, however, difficult in general. 

Example: 6.3 Two Link Planar Robot 

Consider the two link, revolute joint, planar robot from section 
we group the inertia terms appearing in Equation |6.81| as 

0 1 = m 1^1 T 012(^1 + ^02) + Ii + I2 

02 = 

03 = rn2iii C 2 

then we can write the inertia matrix elements as 

dn = 0i + 202 cos(g 2 ) (6.121) 

di 2 = d 2 i = 03 + 0 2 cos(g 2 ) (6.122) 

d 22 = 0 3 (6.123) 


6.4 above. If 


(6.118) 

(6.119) 

( 6 . 120 ) 
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No additional parameters are required in the Christoffel symbols as these are 
functions of the elements of the inertia matrix. The gravitational torques require 


additional parameters, in general. Setting 

©4 = 7711^1+7712^1 (6.124) 

©5 = 7772 f 2 (6.125) 

we can write the gravitational terms, <f> \ and <f > 2 as 

<t>i = 04 ffcos(gi) + 0 5 gcos(gi + q 2 ) (6.126) 

4>2 = @5 cos( 9 i + q 2 ) (6.127) 


Substituting these into the equations of motion it is straightforward to write 
the dynamics in the form (6.1171 where 


Y{q,q,q) = 

9 i cos(g 2 )( 2 gi + 92 ) + sin(q 2 )(qf 
0 cos{q 2 )qi + sin(q 2 )qf 


2gi92) 92 

92 


(6.128) 

geos (91) gcos(9i + 92) 

0 gcos(9i + 92) 


and the parameter vector 0 is given by 


' ©1 ' 


777-lf^i + 7712(^4 + (q 2 ) + II + I 2 

©2 


m 2 t\lc2 

03 

= 

m 2 t\t c2 

©4 


m\t c \ + m 2 l\ 

©5 


7772^2 


(6.129) 


Thus, we have parameterized the dynamics using a five dimensional parameter 
space. Note that in the absence of gravity, as in a SCARA configuration, only 
three parameters are needed. 


6.6 NEWTON-EULER FORMULATION 

In this section, we present a method for analyzing the dynamics of robot ma- 
nipulators known as the Newton-Euler formulation. This method leads 
to exactly the same final answers as the Lagrangian formulation presented in 
earlier sections, but the route taken is quite different. In particular, in the 
Lagrangian formulation we treat the manipulator as a whole and perform the 
analysis using a Lagrangian function (the difference between the kinetic energy 
and the potential energy). In contrast, in the Newton-Euler formulation we 
treat each link of the robot in turn, and write down the equations describing 
its linear motion and its angular motion. Of course, since each link is coupled 
to other links, these equations that describe each link contain coupling forces 
and torques that appear also in the equations that describe neighboring links. 
By doing a so-called forward-backward recursion, we are able to determine all 


216 DYNAMICS 


of these coupling terms and eventually to arrive at a description of the ma- 
nipulator as a whole. Thus we see that the philosophy of the Newton-Euler 
formulation is quite different from that of the Lagrangian formulation. 

At this stage the reader can justly ask whether there is a need for another 
formulation, and the answer is not clear. Historically, both formulations were 
evolved in parallel, and each was perceived as having certain advantages. For 
instance, it was believed at one time that the Newton-Euler formulation is better 
suited to recursive computation than the Lagrangian formulation. However, the 
current situation is that both of the formulations are equivalent in almost all 
respects. Thus at present the main reason for having another method of analysis 
at our disposal is that it might provide different insights. 

In any mechanical system one can identify a set of generalized coordinates 
(which we introduced in Section 6.1 and labeled q) and corresponding gen- 
eralized forces (also introduced in Section 6.1 and labeled r). Analyzing the 
dynamics of a system means finding the relationship between q and r. At this 
stage we must distinguish between two aspects: First, we might be interested 
in obtaining closed-form equations that describe the time evolution of the 
generalized coordinates, such as (6.87 ) for example. Second, we might be inter- 
ested in knowing what generalized forces need to be applied in order to realize 
a particular time evolution of the generalized coordinates. The distinction is 
that in the latter case we only want to know what time dependent function r(-) 
produces a particular trajectory q(-) and may not care to know the general func- 
tional relationship between the two. It is perhaps fair to say that in the former 
type of analysis, the Lagrangian formulation is superior while in the latter case 
the Newton-Euler formulation is superior. Looking ahead to topics beyond the 
scope of the book, if one wishes to study more advanced mechanical phenomena 
such as elastic deformations of the links (i.e., if one no longer assumes rigidity 
of the links), then the Lagrangian formulation is clearly superior. 

In this section we present the general equations that describe the Newton- 
Euler formulation. In the next section we illustrate the method by applying 
it to the planar elbow manipulator studied in Section 6.4 and show that the 
resulting equations are the same as ( 6 . 861 . 

The facts of Newtonian mechanics that are pertinent to the present discussion 
can be stated as follows: 


1. Every action has an equal and opposite reaction. Thus, if body 1 applies a 
force / and torque r to body 2, then body 2 applies a force of — / and 
torque of — r to body 1. 


2. The rate of change of the linear momentum equals the total force applied to 
the body. 


3. The rate of change of the angular momentum equals the total torque applied 
to the body. 
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Applying the second fact to the linear motion of a body yields the relationship 


d(mv ) 
dt 


(6.130) 


where m is the mass of the body, v is the velocity of the center of mass with 
respect to an inertial frame, and / is the sum of external forces applied to the 
body. Since in robotic applications the mass is constant as a function of time, 
(6.1301 can be simplified to the familiar relationship 

ma = / (6.131) 


where a = v is the acceleration of the center of mass. 

Applying the third fact to the angular motion of a body gives 


d(IoLJo) 

dt 


(6.132) 


where / 0 is the moment of inertia of the body about an inertial frame whose 
origin is at the center of mass, ujq is the angular velocity of the body, and To 
is the sum of torques applied to the body. Now there is an essential difference 
between linear motion and angular motion. Whereas the mass of a body is 
constant in most applications, its moment of inertia with respect an inertial 
frame may or may not be constant. To see this, suppose we attach a frame 
rigidly to the body, and let I denote the inertia matrix of the body with respect 
to this frame. Then / remains the same irrespective of whatever motion the 
body executes. However, the matrix /q is given by 


I 0 = RIR t 


(6.133) 


where R is the rotation matrix that transforms coordinates from the body 
attached frame to the inertial frame. Thus there is no reason to expect that Iq 
is constant as a function of time. 

One possible way of overcoming this difficulty is to write the angular motion 
equation in terms of a frame rigidly attached to the body. This leads to 


Iu> + u> x (Id) = 


(6.134) 


where / is the (constant) inertia matrix of the body with respect to the body 
attached frame, w is the angular velocity, but expressed in the body attached 
frame, and r is the total torque on the body, again expressed in the body at- 


tached frame. Let us now give a derivation of (6.1341 to demonstrate clearly 


where the term to x (Iu>) comes from; note that this term is called the gyro- 
scopic term. 

Let R denote the orientation of the frame rigidly attached to the body w.r.t. 


the inertial frame; note that it could be a function of time. Then (6.133) gives 
the relation between I and Iq. Now by the definition of the angular velocity, 
we know that 


RR t = S(lo o) 


(6.135) 
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In other words, the angular velocity of the body, expressed in an inertial frame , 
is given by (6.135 1. Of course, the same vector, expressed in the body attached 
frame, is given by 


io 0 = Ruj,lo = R t lu 0 (6.136) 

Hence the angular momentum, expressed in the inertial frame, is 

h = RIR t Rlo = RIlo (6.137) 

Differentiating and noting that I is constant gives an expression for the rate of 
change of the angular momentum, expressed as a vector in the inertial frame: 

h = RIu + RIio (6.138) 


Now 


S(io 0 ) = RR T , R = S(lo)R (6.139) 

Hence, with respect to the inertial frame, 

h = S(co 0 )RIco + Rlio (6.140) 


With respect to the frame rigidly attached to the body, the rate of change of 
the angular momentum is 

R T h = R t S(lo 0 )RIlo + Iio 
= S(R T loq)Iuj + Iio 

= S(lu)Ilo + Ho = lo x (ho) + Iio (6.141) 


This establishes (6.1341. Of course we can, if we wish, write the same equation 
in terms of vectors expressed in an inertial frame. But we will see shortly that 
there is an advantage to writing the force and moment equations with respect 
to a frame attached to link i, namely that a great many vectors in fact reduce 
to constant vectors, thus leading to significant simplifications in the equations. 

Now we derive the Newton-Euler formulation of the equations of motion of 
an n-link manipulator. For this purpose, we first choose frames 0, . . . , n, where 
frame 0 is an inertial frame, and frame i is rigidly attached to link i for i > 1. 
We also introduce several vectors, which are all expressed in frame i. The first 
set of vectors pertain to the velocities and accelerations of various parts of the 
manipulator. 


«c,i = the acceleration of the center of mass of link i 

a e ^ = the acceleration of the end of link i (i.e., joint z + 1) 

uii = the angular velocity of frame i w.r.t. frame 0 

on = the angular acceleration of frame i w.r.t. frame 0 
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The next several vectors pertain to forces and torques. 


9i 

fi 



the acceleration due to gravity (expressed in frame i ) 
the force exerted by link i — 1 on link i 
the torque exerted by link i — 1 on link i 
the rotation matrix from frame i + 1 to frame i 


The final set of vectors pertain to physical features of the manipulator. Note 
that each of the following vectors is constant as a function of q. In other 
words, each of the vectors listed here is independent of the configuration of the 
manipulator. 


rrii 

Ij 


Ti : ci 
n+i ,ci 

n,i + 1 


the mass of link i 

the inertia matrix of link i about a frame parallel 
to frame i whose origin is at the center of mass of link i 
the vector from joint i to the center of mass of link i 
the vector from joint i + 1 to the center of mass of link i 
the vector from joint i to joint i + 1 


Now consider the free body diagram shown in Figure |6.11| this shows link i 



together with all forces and torques acting on it. Let us analyze each of the 
forces and torques shown in the figure. First, /,■ is the force applied by link 
* — 1 to link i. Next, by the law of action and reaction, link i + 1 applies a 
force of —fi+i to link i, but this vector is expressed in frame i + 1 according to 
our convention. In order to express the same vector in frame i, it is necessary 
to multiply it by the rotation matrix -R* +1 . Similar explanations apply to the 
torques 7$ and — i?* +1 r i+1 . The force is the gravitational force. Since all 
vectors in Figure [6. ll| are expressed in frame i, the gravity vector g* is in general 
a function of i. 
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Writing down the force balance equation for link i gives 

fi - i?- +1 /i + 1 + rriigi = mia Cti (6.142) 

Next we write down the moment balance equation for link i. For this purpose, it 
is important to note two things: First, the moment exerted by a force / about 
a point is given by / x r, where r is the radial vector from the point where 
the force is applied to the point about which we are computing the moment. 
Second, in the moment equation below, the vector does not appear, since 
it is applied directly at the center of mass. Thus we have 

n - R? 1 T i+1 + fi x n, ci - (R^fi+i) x r i+hci (6.143) 
= ai + cot x ( IitUi ) 


Now we present the heart of the Newton-Euler formulation, which consists 
of finding the vectors f n and t\ , . . . , r„ corresponding to a given set of 

vectors q , q , q. In other words, we find the forces and torques in the manip- 
ulator that correspond to a given set of generalized coordinates and first two 
derivatives. This information can be used to perform either type of analysis, 
as described above. That is, we can either use the equations below to find 
the / and r corresponding to a particular trajectory q(-), or else to obtain 
closed-form dynamical equations. The general idea is as follows: Given q,q,q, 
suppose we are somehow able to determine all of the velocities and accelerations 
of various parts of the manipulator, that is, all of the quantities a c +, uii and 


ai . Then we can solve (6.1421 and (6.1431 recursively to find all the forces and 


torques, as follows: First, set f n +i = 0 and r n+ i = 0. This expresses the fact 
that there is no link n + 1. Then we can solve (6.1421 to obtain 


fi — Ri fi+l T lYlidc^i ritj Qj 


(6.144) 


By successively substituting i = n, n — 1, . . . , 1 we find all forces. Similarly, we 


can solve (6.1431 to obtain 


(6.145) 


Ri 71+1 fi Y Ti^ci ( R i fi+l) X Ti+i^d ai UJi X (/jC^^) 


By successively substituting i = rum n — 1, . . . , 1 we find all torques. Note that 
the above iteration is running in the direction of decreasing i. 

Thus the solution is complete once we find an easily computed relation be- 
tween q, q , q and a c +,u>i and ai. This can be obtained by a recursive procedure 
in the direction of increasing i. This procedure is given below, for the case 
of revolute j oint s; the corresponding relation ships for prismatic joints are 
actually easier to derive. 

In order to distinguish between quantities expressed with respect to frame 
i and the base frame, we use a superscript (0) to denote the latter. Thus, for 
example, w* denotes the angular velocity of frame i expressed in frame i, while 
denotes the same quantity expressed in an inertial frame. 
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Now from Section ?? we have that 

w- 0) = uf\+Zi-iqi (6.146) 

This merely expresses the fact that the angular velocity of frame i equals that 
of frame i — \ plus the added rotation from joint i. To get a relation between Ui 
and u>i- 1 , we need only express the above equation in frame i rather than the 
base frame, taking care to account for the fact that u>i and u)i-\ are expressed 
in different frames. This leads to 


LJi — (R\_i) t u>i-\ + biqi 


(6.147) 


where 


hi = {R 1 q) t Zi-i 


(6.148) 


is the axis of rotation of joint i expressed in frame i. 

Next let us work on the angular acceleration a*. It is vitally important to 
note here that 


£ *i = (R'oM 0) 


(6.149) 


In other words, a* is the derivative of the angular velocity of frame i, but 
expressed in frame i. It is not true that a.i = to i\ We will encounter a similar 
situation with the velocity and acceleration of the center of mass. Now we see 
directly from (6.1461 that 


• (o) • (o) , •• , (o) ^ 

+ Zi-iqi + u>\ ' X Zi-xQi 

Expressing the same equation in frame i gives 

LAi ( R j i — 1 ) LXi— i hiQi ~\~ COi X h{C[i 


(6.150) 


(6.151) 


Now we come to the linear velocity and acceleration terms. Note that, in 
contrast to the angular velocity, the linear velocity does not appear anywhere in 
the dynamic equations; however, an expression for the linear velocity is needed 
before we can derive an expression for the linear acceleration. From Section ??, 
we get that the velocity of the center of mass of link i is given by 


„( 0 ) 


„( 0 ) 


+ w- 0) x r\.°2 


(6.152) 


To obtain an expression for the acceleration, we use (??), and note that the 


vector r,- 0 ^; is constant in frame i. Thus 


,(0) 


,( 0 ) 


„(0) 


, ( 0 ) ( 0 )n 

= a e,i- 1 x r i,ci + W i X X r i,a ) 


,( 0 ) 


(6.153) 


Now 


a c,i = (Rq ) T ai°i 


(6.154) 
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Let us carry out the multiplication and use the familiar property 

R{a xb) = ( Ra ) x (Rb) (6.155) 


We also have to account for the fact that a e ,i-i is expressed in frame i — 1 and 
transform it to frame i. This gives 


Qc,i — (Ri— l) T LOj, X T LOi X (iJi X r ) 


(6.156) 


Now to find the acceleration of the end of link i, we can use (6.156 I with r^j+i 
replacing ?y iC j. Thus 

a e ,i = (R\_ l ) T a e ^- 1 +UjX r lji+ i +u,x (w; x r i;i+ i) (6.157) 


Now the recursive formulation is complete. We can now state the Newton-Euler 
formulation as follows. 


1. Start with the initial conditions 


wq — 0, «o — Oj Qc,o — Oj «e,o — 0 


(6.158) 


and solve (6.147), (6.151), (6.157) and (6.1561 (in that order!) to compute 
Wi, a» and a Cj j for i increasing from 1 to n. 

2 . Start with the terminal conditions 


fn- f-1 — 0? 7"n+l — 0 


(6.159) 


and use (6.1441 and (6.1451 to compute /,; and r, for i decreasing from n 
to 1. 
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In this section we apply the recursive Newton-Euler formulation derived in Sec- 
tion 6.6 to analyze the dynamics of the planar elbow manipulator of figure [tTS] 
and show that the Newton-Euler method leads to the same equations as the 
Lagrangian method, namely (6.86). 

We begin with the forward recursion to express the various velocities and 
accelerations in terms of < 71,92 and their derivatives. Note that, in this simple 
case, it is quite easy to see that 


aq = 91 /c, oq = qi k, w 2 = (qi + q 2 )k, a 2 = (91 + q 2 )k (6.160) 

so that there is no need to use (6.1471 and ( |6.151| ). Also, the vectors that are 
independent of the configuration are as follows: 


r\d = 4i i, r 2tCl = (£1 - £ cl )i, n, 2 = i\i (6.161) 

r 2 ,c2 = 42b ^ 3,02 = (4 - 42)b r 2 , 3 = i 2 i (6.162) 
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Forward Recursion link 1 


Using (6.1561 with i = 1 and noting that a 6i o = 0 gives 

a Ci i = qik x 4i i + qik x ( qik X t c \i) 


= 4i<?ij - 4i<?i* = 


■4i9i 

4<?i 

0 


(6.163) 


Notice how simple this computation is when we do it with respect to frame 1. 
Compare with the same computation in frame 0! Finally, we have 


9 1 


= -{Rl) T gj = g 


sin qi 
- COS qi 
0 


(6.164) 


where g is the acceleration due to gravity. At this stage we can economize a 
bit by not displaying the third components of these accelerations, since they 
are obviously always zero. Similarly, the third component of all forces will be 
zero while the first two components of all torques will be zero. To complete the 
computations for link 1, we compute the acceleration of end of link 1. Clearly, 


this is obtained from (6.163) by replacing 4i by t\. Thus 


a e , i = 


-hq\ 

4<?i 


(6.165) 


Forward Recursion: Link 2 


Once again we use (6.156|) and substitute for (o2 from (6.160); this yields 


a c ,2 = (i?i) T a fij i + [(g'i + q 2 )k\ x i c2 i + (91 + q 2 )k x [(91 + q 2 )k x 4 2 *](6.166) 


The only quantity in the above equation which is configuration dependent is 
the first one. This can be computed as 


CRf)V, i 


cos q 2 sin q 2 
— sin q 2 cos q 2 


-lift 

(■iqi 


—i\ q\ cos q 2 + iiqi sin q 2 
liqf sin g2 + ^i9i cos q 2 


Substituting into (6.1661 gives 


— hql cos q- 2 + sin q 2 
l\ q\ sin 92 + hqi cos q 2 


- 42(91 + ? 2) 2 

— 42(91 + 92 ) 


The gravitational vector is 

32 


sin(9i + 92) 
— cos (91 + 92) 


(6.167) 


(6.168) 


(6.169) 


Since there are only two links, there is no need to compute a ey2 . Hence the 
forward recursions are complete at this point. 
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Backward Recursion: Link 2 


Now we carry out the backward recursion to compute the forces and joint 
torques. Note that, in this instance, the joint torques are the externally applied 
quantities, and our ultimate objective is to derive dynamical equations involving 


the joint torques. First we apply (6.1441 with i = 2 and note that fy = 0. This 
results in 


h = rn, 2 a Cy 2 ~ m 2 g2 

t 2 = I 2 a 2 + lo 2 x ( I 2 ui 2 ) -/ 2 X l c2 i 


(6.170) 

(6.171) 


Now we can substitute for u 2 ,a 2 from ( 6. 160 > , and for a Cj 2 from ( 6.168 > . We 
also note that the gyroscopic term equals zero, since both uj 2 and I 2 tu 2 are 
aligned with k. Now the cross product f 2 x £ c2 i is clearly aligned with k and 
its magnitude is just the second component of f 2 . The final result is 

t 2 = I 2 (qi + q 2 )k + \m 2 t\l c2 sin q 2 qf + m 2 t\t c2 cos q 2 q\ 

+m 2 i 2 c2 (q 1 + q 2 ) + m)2£ c2 g cos(< 7 i + q 2 )]k (6.172) 

Since t 2 = r 2 k , we see that the above equation is the same as the second 


equation in (6.871. 


Backward Recursion: Link 1 


To complete the derivation, we apply (6.1441 and (6.145) with i = 1. First, the 
force equation is 

fi = mio Ci i + Rif 2 - TOigi (6.173) 

and the torque equation is 

ti = R\r 2 -h xe Cll i-(Rlf 2 ) x (h-t c i)i (6-174) 

+I\a\ + uq X (Jiuq) 

Now we can simplify things a bit. First, RIt 2 = t 2 , since the rotation matrix 
does not affect the third components of vectors. Second, the gyroscopic term 
is the again equal to zero. Finally, when we substitute for fi from (6.1731 into 
(6.1741, a little algebra gives 

ti = 72 — mia C) i x 4i i + rnigi x £ cl i - (R\f 2 ) 
y*£\i -)- I\i -)- Ii<y± 


(6.175) 


Once again, all these products are quite straightforward, and the only difficult 
calculation is that of Rlf 2 . The final result is: 

Ti = T 2 + mil 2 cl +mi£ c \gcosqi+m 2 £igcosqi+ I\'qi (6.176) 

+ 7712^1 - mi£i£ c2 (qi + q 2 ) 2 smq 2 + m 2 £i£ c2 (qi + q 2 ) cos q 2 


If we now substitute for t± from (6.172) and collect terms, we will get the first 
equation in (6.871; the details are routine and are left to the reader. 
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Problems 


6-1 Consider a rigid body undergoing a pure rotation with no external forces 
acting on it. The kinetic energy is then given as 

K = —{I XX U X + IyyUly + I ZZ U! Z ) 

with respect to a coordinate located at the center of mass and whose 
coordinate axes are principal axes. Take as generalized coordinates the 
Euler angles <j>, 9, and show that the Euler-Lagrange equations of motion 
of the rotating body are 

Ixx^Jx T Ozz Iyy)^y^ z = ^ 

IyyLJy T (I&X Izz)LO Z LJ X ~ 0 

Izz^z T {lyy Ixx^^x^y = 0* 


6-2 Verify the expression (??). 

6-3 Find the moments of inertia and cross products of inertia of a uniform 
rectangular solid of sides a, 6, c with respect to a coordinate system with 
origin at the one corner and axes along the edges of the solid. 

6-4 Given the cylindrical shell shown, show that 

I xx = - mr 2 H md 2 

XX 2 T 12 

I X1 X1 = ^ ml - 2 + ^m£ 2 

I z = mr 2 . 


6-5 Given the inertia matrix D(q) defined by (6.811 show that det D(q) 7^ 0 
for all q. 


6-6 Consider a 3-link cartesian manipulator, 

a) Compute the inertia tensor .7, for each link i = 1,2,3 assuming that 

the links are uniform rectangular solids of length 1, width -y , and 
height |, and mass 1. 

b) Compute the 3x3 inertia matrix D{q) for this manipulator. 

c) Show that the Christoffel symbols Cijk are all zero for this robot. In- 

terpret the meaning of this for the dynamic equations of motion. 

d) Derive the equations of motion in matrix form: 


D{q)q + C{q, q)q + g{q) 


u. 
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6-7 Derive the Euler-Lagrange equations for the planar RP robot in Figure ??. 

6-8 Derive the Euler-Lagrange equations for the planar PR robot in Figure ??. 

6-9 Derive the Euler-Lagrange equations of motion for the three-link RRR 
robot of Figure ??. Explore the use of symbolic software, such as Maple or 
Mathematica, for this problem. See, for example, the Robotica package [?] . 

6-10 For each of the robots above, define a parameter vector, 0, compute the 
Regressor, Y ( q , q, q) and express the equations of motion as 

Y(q,q,q)Q = T (6.177) 

6-11 Recall for a particle with kinetic energy K = | mx 2 , the momentum is 
defined as 


. d,K 
p = mx = —— . 

ax 

Therefore for a mechanical system with generalized coordinates q±, ... ,q n , 
we define the generalized momentum /y. as 

dL 

Pk = 

oqk 

where L is the Lagrangian of the system. With K = | q T D(q)q , and 
L = K — V, prove that 

n 

qkPk = 2 K. 

k = 1 


6-12 There is another formulation of the equations of motion of a mechanical 
system that is useful, the so-called Hamiltonian formulation: Define the 
Hamiltonian function H by 

n 

it = Y Q kpk L - 

k - 1 

a) Show that H = K + V. 

b) Using Lagrange’s equations, derive Hamilton’s equations 

dH 
dpk 
dH 

— b T k 


qk = 

Pk = 


where ry is the input generalized force. 
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c) For two-link manipulator of Figure 6.7 compute Hamiltonian equations 
in matrix form. Note that Hamilton’s equations are a system of first 
order differential equations as opposed to second order system given 
by Lagrange’s equations. 

6-13 Given the Hamiltonian H for a rigid robot, show that 


dH 

dt 


q 


T 


where r is the external force applied at the joints. What are the units of 
dH 7 
dt 
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INDEPENDENT JOINT 

CONTROL 


7.1 INTRODUCTION 

f _l_ 1 he control problem for robot manipulators is the problem of determining 
the time history of joint inputs required to cause the end-effector to execute 
a commanded motion. The joint inputs may be joint forces and torques, or 
they may be inputs to the actuators, for example, voltage inputs to the motors, 
depending on the model used for controller design. The commanded motion is 
typically specified either as a sequence of end-effector positions and orientations, 
or as a continuous path. 

There are many control techniques and methodologies that can be applied 
to the control of manipulators. The particular control method chosen as well 
as the manner in which it is implemented can have a significant impact on the 
performance of the manipulator and consequently on the range of its possible 
applications. For example, continuous path tracking requires a different control 
architecture than does point-to-point control. 

In addition, the mechanical design of the manipulator itself will influence 
the type of control scheme needed. For example, the control problems en- 
countered with a cartesian manipulator are fundamentally different from those 
encountered with an elbow type manipulator. This creates a so-called hard- 
ware/software trade-off between the mechanical structure of the system and 
the architecture/programming of the controller. 

Technological improvements are continually being made in the mechanical 
design of robots, which in turn improves their performance potential and broad- 
ens their range of applications. Realizing this increased performance, however, 
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requires more sophisticated approaches to control. One can draw an analogy to 
the aerospace industry. Early aircraft were relatively easy to fly but possessed 
limited performance capabilities. As performance increased with technological 
advances so did the problems of control to the extent that the latest vehicles, 
such as the space shuttle or forward swept wing fighter aircraft, cannot be flown 
without sophisticated computer control. 

As an illustration of the effect of the mechanical design on the control prob- 
lem, compare a robot actuated by permanent magnet DC motors with gear 
reduction to a direct-drive robot using high-torque motors with no gear reduc- 
tion. In the first case, the motor dynamics are linear and well understood and 
the effect of the gear reduction is largely to decouple the system by reducing the 
inertia coupling among the joints. However, the presence of the gears introduces 
friction, drive train compliance and backlash. 

In the case of a direct-drive robot, the problems of backlash, friction, and 
compliance due to the gears are eliminated. However, the coupling among the 
links is now significant, and the dynamics of the motors themselves may be much 
more complex. The result is that in order to achieve high performance from 
this type of manipulator, a different set of control problems must be addressed. 

In this chapter we consider the simplest type of control strategy, namely, 
independent joint control. In this type of control each axis of the manipulator is 
controlled as a single-input/single-output (SISO) system. Any coupling effects 
due to the motion of the other links is treated as a disturbance. We assume, in 
this chapter, that the reader has had an introduction to the theory of feedback 
control systems up to the level of say, Kuo [?]. 

The basic structure of a single-input/single-output feedback control system 
is shown in Figure |7.1| The design objective is to choose the compensator in 


Disturbance 



Fig. 7.1 Basic structure of a feedback control system. 


such a way that the plant output “tracks” or follows a desired output, given 
by the reference signal. The control signal, however, is not the only input 
acting on the system. Disturbances, which are really inputs that we do not 
control, also influence the behavior of the output. Therefore, the controller 
must be designed, in addition, so that the effects of the disturbances on the 
plant output are reduced. If this is accomplished, the plant is said to ’’reject” 
the disturbances. The twin objectives of tracking and disturbance rejection are 
central to any control methodology. 
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7.2 ACTUATOR DYNAMICS 


In Chapter [G] we obtained the following set of differential equations describing 
the motion of an n degree of freedom robot (cf. Equation (6.61 1) 


D(q)q + C(q,q)q + g(q) = r 


(7.1) 


It is important to understand exactly what this equation represents. Equa- 
tion (7.1 1 represents the dynamics of an interconnected chain of ideal rigid 
bodies, supposing that there is a generalized force r acting at the joints. We 
can assume that the k- th component 7> of the generalized force vector r is a 
torque about the joint axis Zk— i if joint k is revolute and is a force along the 
joint axis Zk-i if joint k is prismatic. This generalized force is produced by 
an actuator, which may be electric, hydraulic or pneumatic. Although Equa- 
tion (7.1 1 is extremely complicated for all but the simplest manipulators, it 
nevertheless is an idealization, and there are a number of dynamic effects that 
are not included in Equation (7.1). For example, friction at the joints is not 
accounted for in these equations and may be significant for some manipulators. 
Also, no physical body is completely rigid. A more detailed analysis of robot 
dynamics would include various sources of flexibility, such as elastic deformation 
of bearings and gears, deflection of the links under load, and vibrations. In this 
section we are interested mainly in the dynamics of the actuators producing 
the generalized force t. We treat only the dynamics of permanent magnet DC- 
motors, as these are the simplest actuators to analyze. Other types of motors, 
in particular AC-motors and so-called Brushless DC-motors are increasingly 
common in robot applications (See [?] for details on the dynamics of AC drives. 

A DC-motor works on the principle that a current carrying conductor in a 
magnetic field experiences a force F = i x </>, where <j> is the magnetic flux and 
i is the current in the conductor. The motor itself consists of a fixed stator 
and a movable rotor that rotates inside the stator, as shown in Figure |7.2| If 
the stator produces a radial magnetic flux <f> and the current in the rotor (also 
called the armature ) is i then there will be a torque on the rotor causing it to 
rotate. The magnitude of this torque is 


T m — K t (f>i a 


(7.2) 


where r m is the motor torque ( N — to), <f> is the magnetic flux (webers), i a is 
the armature current (amperes), and K\ is a physical constant. In addition, 
whenever a conductor moves in a magnetic field, a voltage V b is generated across 
its terminals that is proportional to the velocity of the conductor in the field. 
This voltage, called the back emf. ] will tend to oppose the current flow in the 
conductor. 

Thus, in addition to the torque r m in (7.2 1, we have the back emf relation 

V b = K 2 0Lo m (7.3) 


where V b denotes the back emf (Volts), oj m is the angular velocity of the rotor 
(rad/sec), and K 2 is a proportionality constant. 
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Conductors 
(bonded to rotating 
' armature-iron) 


Rotating flux 
path 


Fig. 7.2 Cross-sectional view of a surface- wound permanent magnet DC motor. 


DC-motors can be classified according to the way in which the magnetic field 
is produced and the armature is designed. Here we discuss only the so-called 
permanent magnet motors whose stator consists of a permanent magnet. In 
this case we can take the flux, <j>, to be a constant. The torque on the rotor is 
then controlled by controlling the armature current, i a . 

Consider the schematic diagram of Figure [73] where 


L 


R 




a/wv 




Fig. 7.3 Circuit diagram for armature controlled DC motor. 
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V = armature voltage 
L = armature inductance 
R = armature resistance 
Vb = back emf 
i a = armature current 
9 m = rotor position (radians) 

T m = generated torque 
Tg = load torque 
<j> = magnetic flux due to stator 


The differential equation for the armature current is then 

dx 

L—jT + Ri a = V-V b . (7.4) 

dt 

Since the flux (j) is constant the torque developed by the motor is 


7~m — K\(f)i a — K^ia 


(7.5) 


where K m is the torque constant in N 


to/ amp. From (7.3 1 we have 


V b 


A 2 0‘~^m — KhOJrn 


d.0 

dt 


(7.6) 


where Kb is the back emf constant. 

We can determine the torque constant of the DC motor using a set of torque- 
speed curves as shown in Figure 7.4 for various values of the applied voltage 



Fig. 7.4 Typical torque-speed curves of a DC motor. 


V. When the motor is stalled, the blocked-rotor torque at the rated voltage is 
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denoted r 0 . Using Equation (7.4 1 with V b = 0 and di a /dt = 0 we have 

Rt o 


V r = Ria = 


K„ 


Therefore the torque constant is 


K m = 


Rtq 

V r 


(7.7) 


(7.8) 


The remainder of the discussion refers to Figure [7(5] consisting of the DC-motor 


0 S T l 



Fig. 7.5 Lumped model of a single link with actuator/gear train. 


in series with a gear train with gear ratio r : 1 and connected to a link of the 
manipulator. The gear ratio r typically has values in the range 20 to 200 or 


more. Referring to Figure 7.5 we set J m = J a + J g , the sum of the actuator 


and gear inertias. The equation of motion of this system is then 
d 0 rn df) rn 


J rr 


dt 2 


B„ 


dt 


= Tm-Tn/r 

= K m i a - re/r 


(7.9) 


the latter equality coming from ( 7.5 1 . In the Laplace domain the three equations 


(7.4), (7.6 1 and (7.9 1 may be combined and written as 


(Ls + R)I a (s) = V{s)-K b sS m (s) 
(• J m S 2 + B m s)Q m (s ) = Kila(s) - Tl{s) / T 


(7.10) 

(7.11) 

The block diagram of the above system is shown in Figure |7.6| The transfer 
function from V(s) to 0 m (s) is then given by (with = 0) 


©m(s) 

U(s) 


I<n 


s [( Ls + R)(J m s + B m ) + K b K m ] 


(7.12) 
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Ti/r 



Fig. 7.6 Block diagram for a DC motor system 


The transfer function from the load torque Te(s)/r to © m (s) is given by (with 
V = 0) 


9m OQ 

M s ) 


—{Ls + R) 


s [( Ls + R)(J m s + B m ) + K b K m \ 


(7.13) 


Frequently it is assumed that the “electrical time constant” ^ is much 

Jrr 


smaller than the “mechanical time constant” 




This is a reasonable as- 


sumption for many electro-mechanical systems and leads to a reduced order 
model of the actuator dynamics. If we now divide numerator and denominator 
of Equations (7.121 and (7.13l by R and neglect the electrical time const ant by 
setting -jjjy equal to zero, the transfer functions in Equations (7.12) and (7.131 
become, respectively, 


and 


9 m (s) 

V(a) 

9 m(s) 

M s ) 


K m /R 


s ( ’F n 


B n 


K b I<m/R) ' 


s(J m (s) + B m + R b R m / R) 


(7.14) 


(7.15) 


In the time domain Equations (7.141 and (7.15l represent, by superposition, 
the second order differential equation 

J m O m (t) + (B m + K b K m /R)0 m (t) = (- K m /R)V{t)- n {t)/r (7.16) 

The block diagram corresponding to the reduced order system ( 7.16| ) is shown 
in Figure [7?7| 

If the output side of the gear train is directly coupled to the link, then the 
joint variables and the motor variables are related by 


0 mk = r k q k ; k = 1,... ,n 


(7.17) 


where r k is the £;-th gear ratio. Similarly, the joint torques r k given by (7.1 1 
and the actuator load torques Ti k are related by 


T( k =T k ; k=l,...,n. 


(7.18) 
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Ti/r 



Fig. 7.7 Block diagram for reduced order system. 


However, in manipulators incorporating other types of drive mechanisms such 
as belts, pulleys, chains, etc., 9 mk need not equal Tkqk ■ hr general one must 
incorporate into the dynamics a transformation between joint space variables 
and actuator variables of the form 

Qk = fk(0 Sl ,...,9 Sn ) ; T ik = /fe(ri,...,r„) (7.19) 

where 9 Sk = 0 mk /r k - 

Example 7.2.1 

Consider the two link planar manipulator shown in Figure [T78] whose actuators 



Fig. 7.8 Two-link manipulator with remotely driven link, 
are both located on link 1. In this case we have, 


qi = 9 Sl 


; qi — 9 Sl + 0 S2 . 


(7.20) 
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Similarly, the joint torques t, and the actuator load torques Tg, are related by 


<3 

II 

rg 2 = n + r 2 . 

(7.21) 

The inverse transformation is then 



to 

II 

►CS 

9s 2 = 92- qi 

(7.22) 

and 



n = r ei ; 

3 

II 

1 

(7.23) 


7.3 SET-POINT TRACKING 

In this section we discuss set-point tracking using a PD or PID compensator. 
This type of control is adequate for applications not involving very fast motion, 
especially in robots with large gear reduction between the actuators and the 
links. The analysis in this section follows typical engineering practice rather 
than complete mathematical rigor. For the following discussion, assume for 
simplicity that 

Qk = 9 Sk = 9 mk /r k and (7.24) 

Tg k = T k . 

Then, for k = 1 ,...,n, the equations of motion of the manipulator can be 
written as 

n n 

Yd jk mj + Y c ijk (q)qiqj + g k {q) = n (7.25) 

j = 1 i,j = 1 

Jrrik 9rrik + {B mk +K bk K mk /R k )e mk = K mk /R k V k ~r k /r k (7.26) 
Combining these equations yields 

( Jm k + “ 2 d kk ((l))O mk + ( B mk + K bk K mk / R k )0 mk = K mk / R k V k — d k (7.27) 

r k 

where d k is defined by 

d k ■ — y ^ ^ Qj T ^ ^ CijkQiQj + 9k- (7.28) 

Tk , , 

1,3 

Note that the coefficient of 9 mk in the above expression is a nonlinear function 
of the manipulator configuration, q. However, large gear reduction, i.e. large 
values of r k , mitigates the influence of this term and one often defines a constant 
average , or effective inertia J e f f k as an approximation of the exact expression 
Jm k + 3 d kk (q). If we further define 

' k 

Beff k =B mk + K bk K mk /R k and u k = K mk /R k V k (7.29) 
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we may write ( 7.26 ) as 

Jeffk ®m k + B e ff k O mk = Mfe — dfc 


(7.30) 


The advantage of this model is its simplicity since the motor dynamics repre- 


sented by ( 7.26 1 are linear. The effect of the nonlinear coupling terms is treated 
as a disturbance dk, which may be small for large gear reduction provided the 
velocities and accelerations of the joints are also small. 

Henceforth we suppress the subscript k representing the particular joint and 
represent ( 7.30 1 in the Laplace domain by the block diagram of Figure 7.9 The 


u +A 






i 

JeffS + Beff 


1 

s 


v > 



Fig. 7.9 Block diagram of simplified open loop system with effective inertia and damp- 
ing. 

set-point tracking problem is now the problem of tracking a constant or step 
reference command 9 d while rejecting a constant disturbance, d. 


7.3.1 PD Compensator 


As a first illustration, we choose a so-called PD-compensator. The resulting 
closed loop system is shown in Figure 7.10 The input U(s) is given by 


d 



Fig. 7.10 Closed loop system with PD-control. 


U(s) = K p (Q d (s) — 0(s)) — K d sQ(s) 


(7.31) 


where K p , Kr, are the proportional (P) and derivative (D) gains, respectively. 

Taking Laplace transforms of both sides of ( 7.30 1 and using the expression 
(7.311 for the feedback control V'(s), leads to the closed loop system 


@m(s) = 


KK, 




(7.32) 
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where f2(s) is the closed loop characteristic polynomial 

fi(s) = Jeffs 2 + (B e f f + KK d )s + KK p (7.33) 


The closed loop system will be stable for all positive values of K p and Kz > and 
bounded disturbances, and the tracking error is given by 


E(s) 


fi d (s)-0 m (s) 

JeffS 2 d~ (Beff + 

fi(s) 


fi(s) 


D{s) 


For a step reference input 


e d ( s ) 


and a constant disturbance 


D(s) 


n d 

s 


D 

s 


(7.34) 


(7.35) 


(7.36) 


it now follows directly from the final value theorem [4] that the steady state 
error e as satisfies 


lim sE(s) 

s — >0 

-D 


(7.37) 

(7.38) 


Since the magnitude D of the disturbance is proportional to the gear reduction 
- we see that the steady state error is smaller for larger gear reduction and can 
be made arbitrarily small by making the position gain K p large, which is to be 
expected since the system is Type 1. 


We know, of course, from (7.281 that the disturbance term D(s) in (7.341 
is not constant. However, in the steady state this disturbance term is just the 
gravitational force acting on the robot, which is constant. The above analysis 
therefore, while only approximate, nevertheless gives a good description of the 
actual steady state error using a PD compensator assuming stability of the 
closed loop system. 


7.3.2 Performance of PD Compensators 


For the PD-compensator given by (7.31 ) the closed loop system is second order 


and hence the step response is determined by the closed loop natural frequency 
w and damping ratio £. Given a desired value for these quantities, the gains 
Kz> and K p can be found from the expression 


{B eff + KK D ) 


KI<„ 


■l 


eff 


■1 


eff 


— ‘2(^UJS 


(7.39) 
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Table 7.1 Proportional and Derivative Gains for the System [7. 11| for Various Values of 
Natural Frequency c j 


Natural 

Proportional 

Derivative 

Frequency (w) 

Gain Kp 

Gain Kp, 

4 

16 

7 

8 

64 

15 

12 

144 

23 


as 


K p = 


u> 2 J, 


eff 


K 


Kd = 


2 B e ff 

K 


(7.40) 


It is customary in robotics applications to take C, = 1 so that the response is 
critically damped. This produces the fastest non-oscillatory response. In this 
context w determines the speed of response. 


Example 7.1 


Consider the second order system of Figure 7.11 


The closed 


►© 


l 

s(s+l) 


o d +. 


©^ Kp + K D s 


Fig. 7.11 Second Order System with PD Compensator 


loop characteristic polynomial is 

p(s) = s 2 -\- (1 + A pf)s + Kp 


(7.41) 


Suppose 0 d = 10 and there is no disturbance (d = 0). With f = 1, the required 
PD gains for various values of u> are shown in Table \7.1\ The corresponding 
step responses are shown in Figure [7. 1 2\ 

Now suppose that there is a constant disturbance d = 40 acting on the sys- 
tem. The response of the system with the PD gains of Table 7.1 are shown in 
Figure 7.13 We see that the steady state error due to the disturbance is smaller 
for large gains as expected, o 


7.3.3 PID Compensator 

In order to reject a constant disturbance using PD control we have seen that 
large gains are required. By using integral control we may achieve zero steady 
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state error while keeping the gains small. Thus, let us add an integral term 
— to the above PD compensator. This leads to the so-called PID control 
law, as shown in Figure |7.14| The system is now Type 2 and the PID control 
achieves exact steady tracking of step (and ramp) inputs while rejecting step 
disturbances, provided of course that the closed loop system is stable. 

With the PID compensator 


C(s) — K p + Kps + 


K, 


the closed loop system is now the third order system 


O m (s) = 


(K^ + Kps + Kj), 


fl 2 (s) 


- 


rs 

n 2 (s) 


D{s) 


where 


(7.42) 


(7.43) 


Cl 2 = J eff a 3 + {B eff + KK D )a 2 + KK p a + KK I (7.44) 


Applying the Routh-Hurwitz criterion to this polynomial, it follows that the 
closed loop system is stable if the gains are positive, and in addition, 


K, < 


(B eff + KK D )K P 
Jeff 


(7.45) 


Example 7.2 To the previous system we have added a disturbance and an 


242 


INDEPENDENT JOINT CONTROL 




Fig. 7.14 Closed loop system with PID control. 


integral control term in the compensator. The step responses are shown in Fig- 
ure 1 7.15\ We see that the steady state error due to the disturbance is removed, 
o 

7.3.4 Saturation 

In theory, one could achieve arbitrarily fast response and arbitrarily small steady 
state error to a constant disturbance by simply increasing the gains in the PD or 
PID compensator. In practice, however, there is a maximum speed of response 
achievable from the system. Two major factors, heretofore neglected, limit the 
achievable performance of the system. The first factor, saturation, is due to 
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limits on the maximum torque (or current) input. Many manipulators, in fact, 
incorporate current limiters in the servo-system to prevent damage that might 
result from overdrawing current. The second effect is flexibility in the motor 
shaft and/or drive train. We illustrate the effects of saturation below and drive 


train flexibility in section 7.5 


Example 7.3 Consider the block diagram of Figure | 7. 1 6\ where the satura- 

Saturation Plant 



Fig. 7.16 Second order system with input saturation. 


tion function represents the maximum allowable input. With PD control and 
saturation the response is below, o 

The second effect to consider is the joint flexibility. Let k r be the effective 
stiffness at the joint. The joint resonant frequency is then uq = ^k r / J e f / . It is 
common engineering practice to limit u> in ( 7.40 ) to no more than half of ui r to 
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avoid excitation of the joint resonance. We will discuss the effects of the joint 
flexibility in more detail in section |7.5[ 

These examples clearly show the limitations of PID-control when additional 
effects such as input saturation, disturbances, and unmodeled dynamics must 
be considered. 


7.4 FEEDFORWARD CONTROL AND COMPUTED TORQUE 


In this section we introduce the notion of feedforward control as a method to 
track time varying trajectories and reject time varying disturbances. 

Suppose that r(t) is an arbitrary reference trajectory and consider the block 
diagram of Figure 7.18 where G(s) represents the forward transfer function of 



Fig. 7.18 Feedforward control scheme. 
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a given system and H(s) is the compensator transfer function. A feedforward 
control scheme consists of adding a feedforward path with transfer function 
F(s) as shown. 

Let each of the three transfer functions be represented as ratios of polyno- 
mials 


G(s) 


g(«) 

P(s ) 


H(s) 


c(s) 

d{s ) 


F(s) 


« 0 ) 

6(s) 


(7.46) 


We assume that G(s) is strictly proper and H(s) is proper. Simple block dia- 
gram manipulation shows that the closed loop transfer function T(s) = is 
given by (Problem 7-9) 


q(s)(c(s)b(s) + a(s)d(s)) 
b(s)(p(s)d(s) + q(s)c(s )) 


(7.47) 


The closed loop characteristic polynomial of the system is then b(s)(p(s)d(s)+ 
q(s)c(s)). For stability of the closed loop system therefore we require that the 
compensator H(s) and the feedforward transfer function F(s) be chosen so that 
the polynomials p(s)d(s) + q(s)c(s) and b(s) are Hurwitz. This says that, in 
addition to stability of the closed loop system the feedforward transfer function 
F(s) must itself be stable. 

If we choose the feedforward transfer function F(s) equal to Q( s y the inverse 
of the forward plant, that is, a(s) = p(s) and b(s) = q(s), then the closed loop 
system becomes 


Q(s)(p(s)d(s) + q{s)c{s))Y(s) = q(s)(p(s)d(s) + q(s)c{s))R(s) (7.48) 

or, in terms of the tracking error E(s) = R(s) — Y(s), 

q(s)(p(s)d(s) + q{s)c(s))E(s) = 0 (7.49) 


Thus, assuming stability, the output y(t) will track any reference trajectory 
r(t). Note that we can only choose F(s) in this manner provided that the 
numerator polynomial q(s) of the forward plant is Hurwitz, that is, as long as 
all zeros of the forward plant are in the left half plane. Such systems are called 
minimum phase. 


If there is a disturbance D(s) entering the system as shown in Figure 7.19 
then it is easily shown that the tracking error E(s) is given by 


E(s) = 


q(s)d(s) 


p(s)d(s) + q(s)c(s) 


D(s ) 


(7.50) 


We have thus shown that, in the absence of disturbances the closed loop system 
will track any desired trajectory r(t) provided that the closed loop system is 
stable. The steady state error is thus due only to the disturbance. 

Let us apply this idea to the robot model of Section |7.3| Suppose that 
6 d {t) is an arbitrary trajectory that we wish the system to track. In this case 


246 


INDEPENDENT JOINT CONTROL 



Fig. 7.19 Feedforward control with disturbance. 



Fig. 7.20 Feedforward compensator for second order system. 


we have from (7.30) G(s ) = 7 ffS 2 +B ffS together with a PD compensator 
H(s ) = I\ p + K os. The resulting system is shown in Figure 7.20 Note that 


G(s) has no zeros at all and hence is minimum phase. Note also that G(s) _1 is 
not a proper rational function. However, since the derivatives of the reference 
trajectory 6 d are known and precomputed, the implementation of the above 
scheme does not require differentiation of an actual signal. It is easy to see 


from ( 7.50 1 that the steady state error to a step disturbance is now given by 


the same expression (7.37 1 independent of the reference trajectory. As before, a 


PID compensator would result in zero steady state error to a step disturbance. 


In the time domain the control law of Figure 7.20 can be written as 


V{t) = 


J, 


eff q d 


Be ff_Qd 


K K 

= f{t) + K D e(t) + K p e{t) 


+ i< D {6 d - e m ) + K p {o d - e m ) (7.5i) 


where f(t) is the feedforward signal 

fit) = (7.52) 

K K 

and e(t) is the tracking error 9 d (t ) — 9{t). Since the forward plant equation is 
Jeff 0m + B eff e m = KVft) — rdit) 

the closed loop error e(f) = 0 m — 9 d satisfies the second order differential equa- 
tion 


= —rdit) 


J e ffe + ( B e ff + KKjj)e + KK p eit ) 


(7.53) 
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Remark 7.6.1 


We note from (7.531 that the characteristic polynomial of the closed loop sys- 
tem is identical to (7.331. The system now however is written in terms of the 
tracking error eft). Therefore, assuming that the closed loop system is stable, 
the tracking error will approach zero asymptotically for any desired joint space 
trajectory in the absence of disturbances, that is, if d = 0. 


Computed Torque Disturbance Cancellation 


We see that the feedforward signal (7.52) results in asymptotic tracking of any 
trajectory in the absence of disturbances but does not otherwise improve the 
disturbance rejection properties of the system. However, although the term 
d(f) in (7.531 represents a disturbance, it is not completely unknown since d 
satisfies (7.28 1. Thus we may consider adding to the above feedforward signal, 
a term to anticipate the effects of the disturbance d(t). Consider the diagram 
of Figure [7.21| Given a desired trajectory, then we superimpose, as shown, the 



Fig. 7.21 Feedforward computed torque compensation. 


term 


d d ~ ^d jk (q d )q d + ^c ijk (q d )q d q d + Qk(q d ) 


(7.54) 


since d d has units of torque, the above feedforward disturbance cancellation 
control is called the method of computed torque. The expression (7.54) thus 


compensates in a feedforward manner the nonlinear coupling inertial, coriolis, 
centripetal, and gravitational forces arising due to the motion of the manipu- 
lator. Although the difference Ad := d d — d is zero o nly in the ideal case of 
perfect tracking ( 9 = 6 d ) and perfect computation of (7.541, in practice, the 


goal is to reduce Ad to a value smaller than d (say, in the usual Euclidean norm 
sense). Hence the computed torque has the advantage of reducing the effects 
of d. Note that the expression (7.54) is in general extremely complicated so 


that the computational burden involved in computing ( 7.54 1 is of major con- 


cern. Since only the values of the desired trajectory need to be known, many of 
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these terms can be precomputed and stored off-line. Thus there is a trade-off 
between memory requirements and on-line computational requirements. This 
has led to the development of table look up schemes to implement (7.541 and 
also to the development of computer programs for the automatic generation 
and simplification of manipulator dynamic equations. 


7.5 DRIVE TRAIN DYNAMICS 

In this section we discuss in more detail the problem of joint flexibility. For many 
manipulators, particularly those using harmonic drivesj^for torque transmission, 
the joint flexibility is significant. In addition to torsional flexibility in the gears, 
joint flexibility is caused by effects such as shaft windup, bearing deformation, 
and compressibility of the hydraulic fluid in hydraulic robots. 

Consider the idealized situation of Figure [T22] consisting of an actuator con- 


Q\ I, M 



Fig. 7.22 Idealized model to represent joint flexibility. 

nected to a load through a torsional spring which represents the joint flexibility. 
For simplicity we take the motor torque u as input. The equations of motion are 
easily derived using the techniques of Chapter [6j with generalized coordinates 
9 1 and 0 m , the link angle, and the motor angle, respectively, as 

Ji9 e + B t 9 e + k{9 e - 9 m ) = 0 (7.55) 

JmOm + B m e m - k{9( - 6 m ) = u (7.56) 


1 Harmonic drives are a type of gear mechanism that are very popular for use in robots due to 
their low backlash, high torque transmission and compact size. However, they also introduce 
unwanted friction and flexibility at the joints. 
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where J m are the load and motor inertias, Bg and B m are the load and 
motor damping constants, and u is the input torque applied to the motor shaft. 
In the Laplace domain we can write this as 


where 


P£(s)9((s) = kO m (s) 

Pm(s)O m (s) = kOt(s)+U(s) 


Pt(s) = Jes 2 + B(S + k 
Pm{s) = J m s 2 + B m s + k 


(7.57) 

(7.58) 


(7.59) 

(7.60) 


This system is represented by the block diagram of Figure [7T23J 



Fig. 7.23 


Block diagram for the system ( 7.57 H 7.58 1. 


The output to be controlled is, of course, the load angle Op. The open loop 
transfer function between U and is given by 

= k (T.ei) 

U(s) pi{s)Pm{s)-k> 1 J 

The open loop characteristic polynomial is 

JiJmS A + ( JeB m + J m i?i)s 3 + (k(Je + Jm) + B^B m )s 2 + k(B( + B m )s (7.62) 

If the damping constants B ? and B m are neglected, the open loop characteristic 
polynomial is 

JtJmS^ + k(Je + J m )s 2 (7.63) 

which has a double pole at the origin and a pair of complex conjugate poles 
at s = ±ju> where uj 2 = k ( j- + y- ) . Assuming that the open loop damping 
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constants Be and B m are small, then the open loop poles of the system (7.57)- 
(7.581 will be in the left half plane near the poles of the undamped system. 

Kr>s. At this point 
sors are placed on 
3 D-compensator is 
he motor variables 
: block diagram of 
; root locus for the 



Fig. 7.24 PD-control with motor angle feedback, 
closed loop system in terms of Kp is shown in Figure |7.25| 



Fig. 7.25 Root locus for the system of Figure [7. 24| 

We see that the system is stable for all values of the gain Ke> but that 
the presence of the open loop zeros near the jui axis may result in poor over- 
all performance, for example, undesirable oscillations with poor settling time. 
Also the poor relative stability means that disturbances and other unmodeled 
dynamics could render the system unstable. 

If we measure instead the load angle 8g, the system with PD control is 
represented by the block diagram of Figure [7.26| The corresponding root locus 
is shown in Figure [7.27| In this case the system is unstable for large Ke>. The 
critical value of Kp, that is, the value of Kd for which the system becomes 



SPACE DESIGN 


251 


Fig. 7.26 PD-control with load angle feedback. 



Fig. 7.27 Root locus for the system of Figure 7.22. 


unstable, can be found from the Routh criterion. The best that one can do in 
this case is to limit the gain Kp so that the closed loop poles remain within 
the left half plane with a reasonable stability margin. 


Example 7.4 

eters (see [1]) 


Suppose that the system (1.55)- (1.56) has the following param- 


k = 0.8Nm/rad B m = 0.015Nms/rad (7.64) 

J m = 0.0004 Nms 2 /rad Bg = O.ONms/rad 

= 0.00047Vto 2 /rad 


If we implement a PD controller Kjj(s + a) then the response of the system 
with motor ( respectively , load) feedback is shown in Figure 1.28 ( respectively , 
Figure 1.29). o 


7.6 STATE SPACE DESIGN 

In this section we consider the application of state space methods for the control 
of the flexible joint system above. The previous analysis has shown that PD 
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Fig. 7.28 Step response PD-control with motor angle feedback. 


control is inadequate for robot control unless the joint flexibility is negligible or 
unless one is content with relatively slow response of the manipulator. Not only 
does the joint flexibility limit the magnitude of the gain for stability reasons, 
it also introduces lightly damped poles into the closed loop system that result 
in unacceptable oscillation of the transient response. We can write the system 
( 7.55 )-( 7.56 ) in state space by choosing state variables 


xi = Ot x 2 = Oe 

23 = 6 m X 4 = 6 m- 


In terms of these state 


variables the system ( 7.55 )-( 7.56) becomes 


x 1 
22 
x-i 
x 4 


x 2 


B f 


-~r x 1 — r x z + ~r x 3 

J? Ji j£ 


24 

k Be k 1 

21 - —24 - —23 + — u 
Jim J m Jm Jm 


which, in matrix form, can be written as 


(7.65) 


(7.66) 


(7.67) 


2 


Ax + bu 


(7.68) 
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Fig. 7.29 Step response - PD control with load angle feedback. 


where 


A = 


' 0 

1 

0 

0 ' 


0 

k 

B e 

k 

0 


0 

Jl 

Ji 

Jl 

; b = 

0 

0 

0 

1 

0 

k 

0 

k 



i 

- Jm 

Jm 

Jm - 


- Jm - 


(7.69) 


If we choose an output y(t), say the measured load angle 0((t), then we have 
an output equation 


where 


y = Xi = c x 


c T = [1,0, 0,0], 


(7.70) 


(7.71) 


The relationship between the state space form ( 7.68 )-( 7.70) and the transfer 


function defined by (7.611 is found by taking Laplace transforms of ( 7.68 1 -( 7.70 1 
with initial conditions set to zero. This yields 


G(s) = 


Qe(s) F(s) 


= c 1 ( sI-A)~ L b 


(7.72) 


U(s) U(s) 

where I is the n x n identity matrix. The poles of G(s) are eigenvalues of the 


matrix A. In the system (7.681 (7.70) the converse holds as well, that is, all of 
the eigenvalues of A are poles of G(s). This is always true if the state space 
system is defined using a minimal number of state variables [8]. 


254 INDEPENDENT JOINT CONTROL 


7.6.1 State Feedback Compensator 


Given a linear system in state space form, such as (7.68), a linear state feed- 
back control law is an input u of the form 


u(t) = —k 1 x 

4 


(7.73) 


= 


where ki are constants and r is a reference input. In other words, the control 
is determined as a linear combination of the system states which, in this case, 
are the motor and load positions and velocities. Compare this to the previous 
PD-control, which was a function either of the motor position and velocity or 


of the load position and velocity, but not both. The coefficients ki in (7.731 are 


the gains to be determined. If we substitute the control law (7.731 into (7.681 
we obtain 


= (A — bk T )x + br. 


(7.74) 


Thus we see that the linear feedback control has the effect of changing the poles 
of the system from those determined by A to those determined by A — bk T . 

In the previous PD-design the closed loop pole locations were restricted to 
lie on the root locus shown in Figure |7.25| or |7.27| Since there are more free 
parameters to choose in (7.731 than in the PD controller, it may be possible to 


achieve a much larger range of closed loop poles. This turns out to be the case 


if the system ( 7.68 ) satisfies a property known as controllability. 


(i) Definition 7.4.2 

A linear system is said to be completely state-controllable, or controllable 

for short, if for each initial state x(to) and each final state x(tf) there is a control 
input t — > u{t ) that transfers the system from x(to) at time t — o to x(tf) at 
time tf. 

The above definition says, in essence, that if a system is controllable we can 
achieve any state whatsoever in finite time starting from an arbitrary initial 
state. To check whether a system is controllable we have the following simple 
test. 


(ii) Lemma 7.4.3 


A linear system of the form (7.681 is controllable if and only if 


det[6, Ab, A 2 b , . . . , A n 1 b\ ^ 0. 


(7.75) 


The nxn matrix [6, Ab , . . . , A n ~ 1 b\ is called the controllability matrix for 
the linear system defined by the pair (A, b). The fundamental importance of 
controllability of a linear system is shown by the following 
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(iii) Theorem 7.4.4 


Let a(x) = s n + a n s n 1 + • • • + ct 2 S + aq be an arbitrary polynomial of degree 


n. Then there exists a state feedback control law ( 7.73 I such that 


det(sJ — A + bk T ) = a(s) 


(7.76) 


if and only if the system (7.68) is controllable. 

This fundamental result says that, for a controllable linear system, we may 
achieve arbitrarj0 closed loop poles using state feedback. Returning to the 
specific fourth-order system (7.69) we see that the system is indeed controllable 


since 


det[6, Ab, A 2 b, A 3 b] = 


T 4 7 2 
J m d £ 


(7.77) 


which is never zero since k > 0. Thus we can achieve any desired set of closed 
loop poles that we wish, which is much more than was possible using the pre- 
vious PD compensator. 

There are many algorithms that can be used to determine the feedback gains 
in (7.73) to achieve a desired set of closed loop poles. This is known as the 
pole assignment problem. In this case most of the difficulty lies in choosing 
an appropriate set of closed loop poles based on the desired performance, the 
limits on the available torque, etc. We would like to achieve a fast response 
from the system without requiring too much torque from the motor. One way 
to design the feedback gains is through an optimization procedure. This takes 
us into the realm of optimal control theory. For example, we may choose as our 
goal the minimization of the performance criterion 


J = 


(x t Qx + Ru 2 )dt 


(7.78) 


where Q is a given symmetric, positive definite matrix and R > O. 


Choosing a control law to minimize (7.781 frees us from having to decide 


beforehand what the closed loop poles should be as they are automatically 


dictated by the weighting matrices Q and R in (7.781. It is shown in optimal 


control texts that the optimum linear control law that minimizes (7.781 is given 
as 


u = -k. T x (7.79) 

where 

k. = R~ 1 b T P (7.80) 


2 Since the coefficients of the polynomial a(s) are real, the only restriction on the pole locations 

is that they occur in complex conjugate pairs. 
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and P is the (unique) symmetric, positive definite n x n matrix satisfying the 
so-called matrix Algebraic Riccatti equation 


A 1 P + PA — PbR~ 1 b r P + Q = 0. 


(7.81) 


The control law (7.791 is referred to as a Linear Quadratic (LQ) Optimal 
Control, since the performance index is quadratic and the control system is 
linear. 


(iv) Example 7.4.5 


For illustration purposes, let Q and R in ( 7.78 I be given as Q = diag{100, 0.1, 100, 0.1} 
and R = 100. This puts a relatively large weighting on the position variables 
and control input with smaller weighting on the velocities of the motor and 
load. Figure 7.30 shows the optimal gains that result and the response of this 



Fig. 7.30 Step response-Linear, Quadratic-Optimal (LQ) state feedback control. 


LQ-optimal control for the system (7.661 with a unit step reference input r. 


7.6.2 Observers 


The above result is remarkable; however, in order to achieve it, we have had to 
pay a price, namely, the control law must be a function of all of the states. In 
order to build a compensator that requires only the measured output, in this 
case Oe, we need to introduce the concept of an observer. An observer is a state 
estimator. It is a dynamical system (constructed in software) that attempts to 
estimate the full state x(t) using only the system model (7.681-7.70) and the 
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measured output y{t). A complete discussion of observers is beyond the scope 
of the present text. We give here only a brief introduction to the main idea of 
observers for linear systems. 


Assuming that we know the parameters of the system ( 7.68 1 we could sim- 


ulate the response of the system in software and recover the value of the state 
x(t) at time t from the simulation. We could then use this simulated or esti- 


mated state, call in x(t), in place of the true state in (7.791. However, since 
the true initial condition x(to) for (7.681 will generally be unknown, this idea 
is not feasible. However the idea of using the model of the system (7.681 is a 


good starting point to construct a state estimator in software. Let us, therefore, 
consider an estimate x(t) satisfying the system 


= Ax + bu + £(y — c T x). 


(7.82) 


Equation ( 7.82 1 is called an observer for ( 7.68|) and represents a model of the 
system (7.681 with an additional term i(y — c 1 x). This additional term is a 
measure of the error between the output y(t) = c T x{t ) of the plant an d the 
estimate of the output, c T x(t). Since we know the coefficient matrices in (7.821 


and can measure y directly, we can solve the above system for x(t) starting from 
any initial condition, and use this x in place of the true state x in the feedback 
law (7.791. The additional term £ in (7.821 is to be designed so that x — > x as 
t — > oo, that is, so that the estimated state converges to the true (unknown) 
state independent of the initial condition x(to). Let us see how this is done. 

Define e(t) = x — x as the estimation error. Combining (7.681 and ( 7.82| ), 
since y = c T x , we see that the estimation error satisfies the system 


= (A - lc T )t 


(7.83) 


From ( 7.83 ) we see that the dynamics of the estimation error are determined 
by the eigenvalues of A — £c T . Since l is a design quantity we can attempt to 
choose it so that e(t) — ■> 0 as t — > oo, in which case the estimate x converges 
to the true state x. In order to do this we obviously want to choose £ so that 
the eigenvalues of A — ic T are in the left half plane. This is similar to the pole 
assignment problem considered previously. In fact it is dual, in a mathematical 
sense, to the pole assignment problem. It turns out that the eigenvalues of 
A — ic T can be assigned arbitrarily if and only if the pair (A, c) satisfies the 
property known as observability. Observability is defined by the following: 


(i) Definition 7.4.6 

A linear system is completely observable, or observable for short, if every 
initial state x(t 0 ) can be exactly determined from measurements of the output 
y(t) and the input u(t) in a finite time interval to < t < t f . 

To check whether a system is observable we have the following 
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(ii) Theorem 7.4.7 

The pair (A, c) is observable if and only if 


det c,A T c,...,A t c ^ 0. 


(7.84) 


The n x n matrix [c T , c T A T , . . , 
the pair (A,c T ). In the system ( 7.68 )-( 7. 70 > above we have that 


A T ] is called the observability matrix for 


det 


c, A 7 c, A 1 c, A r c 


n 


(7.85) 


and hence the system is observable. A result known as the Separation Prin- 


ciple says that if we use the estimated state in place of the true state in ( 7.79 1, 


then the set of closed loop poles of the system will consist of the union of the 
eigenvalues of A — £c T and the eigenvalues of A — bk T . As the name suggests 
the Separation Principle allows us to separate the design of the state feedback 


control law (7.79) from the design of the state estimator (7.821. A typical pro- 


cedure is to place the observer poles to the left of the desired pole locations of 
A — bk T . This results in rapid convergence of the estimated state to the true 
state, after which the response of the system is nearly the same as if the true 


state were being used in (7.791. 


The result that the closed loop poles of the system may be placed arbitrarily, 
under the assumption of controllability and observability, is a powerful theoret- 
ical result. There are always practical considerations to be taken into account, 
however. The most serious factor to be considered in observer design is noise in 
the measurement of the output. To place the poles of the observer very far to 
the left of the imaginary axis in the complex plane requires that the observer 
gains be large. Large gains can amplify noise in the output measurement and 
result in poor overall performance. Large gains in the state feedback control law 


( 7.79 1 can result in saturation of the input, again resulting in poor performance. 


Also uncertainties in the system parameters, nonlinearities such as a nonlinear 
spring characteristic or backlash, will reduce the achievable performance from 
the above design. Therefore, the above ideas are intended only to illustrate 
what may be possible by using more advanced concepts from control theory. 
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Problems 


7-1 

7-2 

7-3 

7-4 

7-5 

7-6 


7-7 


7-8 


7-9 

7-10 


7-11 


7-12 


Using block diagram reduction techniques derive the transfer functions 


(7.121 and (7.131. 


Derive the transfer functions for the reduced order model ( 7. 14 )-( 7. 15 ) . 


Derive Equations (7.321, (7.331 and (7.341. 


Derive Equations ( 7.43 )-( 7.44 1 . 


Derive Equations (7.611, (7.621, and (7.63 1 . 


Given the state space model (7138]) show that the transfer function 
G(s) = c T (sI—A)~ 1 b 


is identical to (7.61 1 . 


Search the control literature (e.g., [8]) and find two or more algorithms for 
the pole assignment problem for linear systems. Design a state feedback 


control law for (7.681 using the parameter values given in Example 7.4.1 


so that the poles are at s = —10. Simulate the step response. How does 
it compare to the response of Example 7.4.1? How do the torque profiles 
compare? 


Design an observer for the system (7.68) using the parameter values of 


Example 7.4.1. Choose reasonable locations for the observer poles. Sim- 
ulate the combined observer/state feedback control law using the results 
of Problem 7-7. 


Derive (7.771 and (7.851. 


Given a tlrree-link elbow type robot, a three-link SCARA robot and a 
three-link cartesian robot, discuss the differences in the dynamics of each 
type of robot as they impact the control problem. Discuss the nature of 
the coupling nonlinearities, the effect of gravity, and inertial variations 
as the robots move about. For which manipulator would you expect PD 
control to work best? worst? 


Consider the two-link cartesian robot of Example 6.4.1. Suppose that each 
joint is actuated by a permanent magnet DC-motor. Write the complete 
dynamics of the robot assuming perfect gears. 

Carry out the details of a PID control for the two-link cartesian robot of 
Problem El Note that the system is linear and the gravitational forces 
are configuration independent. What does this say about the validity of 
this approach to control? 
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7-13 Simulate the above PID control law. Choose reasonable numbers for the 
masses, inertias, etc. Also place reasonable limits on the magnitude of 
the control input. Use various methods, such as root locus, Bode plots, 
etc. to design the PID gains. 

7-14 Search the control literature (e.g., [6]) to find out what is meant by inte- 
grator windup. Did you experience this problem with the PID control 


law of Problem 13? Find out what is meant by anti-windup (or anti- 
reset windup). Implement the above PID control with anti-reset windup. 
Is the response better? 


7-15 Repeat the above analysis and control design (Problems 11 - 14 for the 
two-link elbow manipulator of Example 6.4.2. Note that you will have to 
make some assumptions to arrive at a value of the effective inertias J e / / . 


7-16 Repeat Problem 15 for the two-link elbow manipulator with remote drive 


of Example 6.4.3. 

7-17 Include the dynamics of a permanent magnet DC-motor for the system 


( 7.55 >-( 7.56 > . What can you say now about controllability and observ- 


ability of the system? 


7-18 Choose appropriate state variables and write the system ( 7.10 >-( 7.11 ) in 
state space. What is the order of the state space? 


7-19 Repeat Problem 7-18 for the reduced order system (7.161. 


7-20 Suppose in the flexible joint system represented by ( 7.55 )-( 7.56 1 the fol- 
lowing parameters are given 


Ji = 10 B , 
Jm. = 2 


= 1 k = 100 
B m = 0.5 


(a) Sketch the open loop poles of the transfer functions (7.61). 

(b) Apply a PD compensator to the system ( 7.61 ). Sketch the root locus 
for the system. Choose a reasonable location for the compensator 
zero. Using the Routh criterion find the value of the compensator 
gain K when the root locus crosses the imaginary axis. 

7-21 One of the problems encountered in space applications of robots is the 
fact that the base of the robot cannot be anchored, that is, cannot be 
fixed in an inertial coordinate frame. Consider the idealized situation 


shown in Figure 7.31 consisting of an inertia J\ connected to the rotor of 
a motor whose stator is connected to an inertia J 2 . For example, J\ could 
represent the space shuttle robot arm and J 2 the inertia of the shuttle 
itself. The simplified equations of motion are thus 


J l9i 
J2<i2 


T 
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Fig. 7.31 Coupled Inertias in Free Space 


Write this system in state space form and show that it is uncontrollable. 
Discuss the implications of this and suggest possible solutions. 

7-22 Given the linear second order system 


±1 


' 1 -3 ' 


Xi 


1 ' 


. *2 . 


1 -2 


. *2 . 

+ 

-2 

u 


find a linear state feedback control u = k\X\ + k 2X2 so that the closed 
loop system has poles at s = — 2, 2. 

7-23 Repeat the above if possible for the system 


X 1 


' -1 O' 


Xi 


' 0 ' 


_ X2 


0 2 


. x 2 

+ 

1 

U 


Can the closed loop poles be placed at -2? 

Can this system be stabilized? Explain. 

[Remark: The system of Problem 7-23 is said to be stabilizable, which 
is a weaker notion than controllability.] 

7-24 Repeat the above for the system 


±1 


'+10' 


Xi 


' 0 ' 


_ X 2 


0 2 


. X2 

+ 

1 

U 


7.18 


Suppose that G(s) = 


1 

2s 2 +s' 


7-25 Consider the block diagram of Figure 

and suppose that it is desired to trade a reference signal r(t) = sin (t) 
cos(2£). If we further specify that the closed loop system should have 
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a natural frequency less than 10 radians with a damping ratio greater 
than 0.707, compute an appropriate compensator C(s) and feedforward 
transfer function F(s). 



8 


MULTIVARIABLE 

CONTROL 


8.1 INTRODUCTION 


An the previous chapter we discussed techniques to derive a control law for 
each joint of a manipulator based on a single-input/single-output model. Cou- 
pling effects among the joints were regarded as disturbances to the individual 
systems. In reality, the dynamic equations of a robot manipulator form a com- 
plex, nonlinear, and multivariable system. In this chapter, therefore, we treat 
the robot control problem in the context of nonlinear, multivariable control. 
This approach allows us to provide more rigorous analysis of the performance 
of control systems, and also allows us to design robust and adaptive nonlinear 
control laws that guarantee stability and tracking of arbitrary trajectories. 

We first reformulate the manipulator dynamic equations in a form more 
suitable for the discussion to follow. Recall the robot equations of motion 


(7.251 and (7.261 


^2d jk (q)qj + ^2 Cijk(q)qiqj + 4>k = n ( 8 . 1 ) 

3 = 1 ij = 1 

J rrik @my. + B k d mk = K mk /R k V k - r k /r k . (8.2) 


where B k = B mk + Kb k K mk / R k . Multiplying (8.2 1 by r k and using the fact 
that 


= r k q k 


(8.3) 
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we write Equation (8.2 1 as 


T k B k q k — ^k^Amk / RV k T k 


(8.4) 


Substituting (8.4 1 into (8.1 1 yields 


rlJm k qk + '^2dj k q j + ^2 c ijk qiqj + rlB k q k + (f>k = r k ^ L V k . (8.5) 


j — 1 i,j = 1 

In matrix form these equations of motion can be written as 
M(q)q + C(q,q)q + Bq + g(q) = u 


R 


( 8 . 6 ) 


where M(q) = D(q) + J where J is a diagonal matrix with diagonal elements 
r k J mk . The vector g(q) and the matrix C(q 1 q) are defined by (6.61) and (6.621, 
respectively, and the input vector u has components 


Kn 


u k = r k - 


Rk 


-V k . 


Note that u k has units of torque. 

Henceforth, we will take B = 0 for simplicity in Equation (8.6 1 and use this 
equation for all of our subsequent development. We leave it as an exercise for the 
reader (cf:Problem X) to show that the properties of passivity, skew-symmetry, 
bounds on the inertia matrix and linearity in the parameters continue to hold 
for the system (8.6 1 . 


8.2 PD CONTROL REVISITED 

It is rather remarkable that the simple PD-control scheme for set-point control 
of rigid robots that we discussed in Chapter [7] can be rigorously shown to work 
in the general caseQ An independent joint PD-control scheme can be written 
in vector form as 


u = Kpq — Kjjq 


(8.7) 


where q = q d — q is the error between the desired joint displacements q d and 
the actual joint displacements q , and ATp, Kp are diagonal matrices of (pos- 
itive) proportional and derivative gains, respectively. We first show that, in 
the absence of gravity, that is, if g is zero in (8.6), the PD control law (8.7 1 
achieves asymptotic tracking of the desired joint positions. This, in effect, re- 
produces the result derived previously, but is more rigorous, in the sense that 
the nonlinear equations of motion (8.1 1 are not approximated by a constant 
disturbance. 


1 The reader should review the discussion on Lyapunov Stability in Appendix C. 
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To show that the above control law achieves zero steady state error consider 
the Lyapunov function candidate 


V = l/2q d M(q)q + l/2q 2 K P q. 


( 8 . 8 ) 


The first term in (8.8 1 is the kinetic energy of the robot and the second term 


accounts for the proportional feedback Kpq. Note that V represents the total 
kinetic energy that would result if the joint actuators were to be replaced by 
springs with stiffnesses represented by Kp and with equilibrium positions at q d . 
Thus V is a positive function except at the “goal” q = q d , q = 0, at which point 

V is zero. The idea is to show that along any motion of the robot, the function 

V is decreasing to zero. This will imply that the robot is moving toward the 
desired goal configuration. 

To show this we note that, since J and q d are constant, the time derivative 
of V is given by 


V = q 1 M(q)q + 1/2^ D(q)q - q 1 K P q. 


(8.9) 


Solving for M(q)q in (8.6) with g(q) = 0 and substituting the resulting expres- 

( 8 . 10 ) 


sion into (8.9 1 yields 


V = q T (u-C(q,q)q) + l/2q T D(q)q-q T K P q 
= q T { u - K P q ) + 1/2 q T (D(q) - 2 C{q, q))q 
= q T (u — K P q ) 


where in the last equality we have used the fact (Theorem 6.3.1) that D — 2 C 
is skew symmetric. Substituting the PD control law ( |8.7| ) for u into the above 
yields 


V = —q T K D q < 0. 


( 8 . 11 ) 


The above analysis shows that V is decreasing as long as q is not zero. This, 
by itself is not enough to prove the desired result since it is conceivable that the 
manipulator can reach a position where q = 0 but q ^ q d . To show that this 
cannot happen we can use LaSalle’s Theorem (Appendix C). Suppose V = 0. 


Then (8.11 1 implies that q = 0 and hence q = 0. From the equations of motion 


with PD-control 


M(q)q + C(q,q)q = - K P q-K D q 


must then have 


0 = -K P q 

which implies that q = 0, q = 0. LaSalle’s Theorem then implies that the 
equilibrium is asymptotically stable. 
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In case there are gravitational terms present in (8.6 1 Equation (8.10) must 
be modified to read 


V = q T (u- g(q) - K P q). 


(8.12) 


The presence of the gravitational term in ( 8.12| ) means that PD control alone 
cannot guarantee asymptotic tracking. In practice there will be a steady state 
error or offset. Assuming that the closed loop system is stable the robot con- 
figuration q that is achieved will satisfy 


K P {q d -q) = g(q). 


(8.13) 


The physical interpretation of ( |8.13[ ) is that the configuration q must be such 
that the motor generates a steady state “holding torque” K P (q d — q) sufficient 
to balance the gravitational torque g(q). Thus we see that the steady state 
error can be reduced by increasing the position gain Kp. 

In order to remove this steady state error we can modify the PD control law 
as 


= K P q- K D q + g(q). 


(8.14) 


The modified control law (8.141, in effect, cancels the effect of the gravitational 
terms and we achieve the same Equation (8.11l as before. The control law 
(8.141 requires microprocessor implementation to compute at each instant the 
gravitational terms g{q) from the Lagrangian equations. In the case that these 
terms are unknown the control law (8.141 cannot be implemented. We will say 
more about this and related issues later. 


8.3 INVERSE DYNAMICS 


We now consider the application of more complex nonlinear control techniques 
for trajectory tracking of rigid manipulators. Consider again the dynamic equa- 


tions of an n-link robot in matrix form from (8.6) 


M{q)q + C(q,q)q + g{q) = u. 


(8.15) 


The idea of inverse dynamics is to seek a nonlinear feedback control law 

u = f(q,q,t) ( 8 - 16 ) 


which, when substituted into (8.151, results in a linear closed loop system. For 


general nonlinear systems such a control law may be quite difficult or impossible 


to find. In the case of the manipulator dynamic equations (8.151, however, the 
problem is actually easy. By inspecting (8.151 we see that if we choose the 
control u according to the equation 


u = M(q)a q + C(q, q)q + g(q) 


(8.17) 
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then, since the inertia matrix M is invertible, the combined system ( 8. 15 1-( 8. 17 ) 
reduces to 


Q = 


(8.18) 


The term a q represents a new input to the system which is yet to be chosen. 


Equation (8.181 is known as the double integrator system as it represents n 


uncoupled double integrators. The nonlinear control law (8.171 is called the 
inverse dynamics confroF] and achieves a rather remarkable result, namely that 
the “new” system (8.18|) is linear, and decoupled. This means that each input 


a qk can be designed to control a scalar linear system. Moreover, assuming 
that a qk is a function only of qk and its derivatives, then a qk will affect qk 
independently of the motion of the other links. 

Since a qk can now be designed to control a linear second order system, the 
obvious choice is to set 


a q = —K 0 q — Kiq + r (8.19) 

where Kq and K\ are diagonal matrices with diagonal elements consisting of 
position and velocity gains, respectively. The closed loop system is then the 
linear system 


q + K 1 q + K 0 q = r. (8.20) 

Now, given a desired trajectory 

t - (q d (t),q d (t)). 

if one chooses the reference input r(t) a^j 

r{t) = q d (t) + K 0 q d (t) + Kiq d (t) 

then the tracking error e(t) = q — q d satisfies 

e(t) + A'ie(t) + K 0 e(t) = 0. (8.23) 

A simple choice for the gain matrices K 0 and Ki is 

K o = diag{w?,...,c4} (8.24) 

I<! = diag{2wi,...,2w„} 

which results in a closed loop system which is globally decoupled, with each joint 
response equal to the response of a critically damped linear second order system 
with natural frequency u>i- As before, the natural frequency u>i determines the 


( 8 . 21 ) 

( 8 . 22 ) 


2 We should point out that in the research literature the control law ( |8.17| ) is frequently called 
computed torque as well. 

3 Compare this with the feedforward expression (7.51 ). 
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speed of response of the joint, or equivalently, the rate of decay of the tracking 
error. 

The inverse dynamics approach is extremely important as a basis for control 
of robot manipulators and it is worthwhile trying to see it from alternative 
viewpoints. We can give a second interpretation of the control law (8.171 as 
follows. Consider again the manipulator dynamic equations (8.151. Since M(q) 
is invertible for q £ R n we may solve for the acceleration q of the manipulator 
as 


q = M 1 {u- C(q,q)q~ g(q)}- (8.25) 

Suppose we were able to specify the acceleration as the input to the system. 
That is, suppose we had actuators capable of producing directly a commanded 
acceleration (rather than indirectly by producing a force or torque). Then the 
dynamics of the manipulator, which is after all a position control device, would 
be given as 


q(t) = a q {t) 


(8.26) 


where a q (t) is the input acceleration vector. This is again the familiar double 
integrator system. Note that (8.26 ) is not an approximation in any sense; rather 


it represents the actual open loop dynamics of the system provided that the 


acceleration is chosen as the input. The control problem for the system (8.261 
is now easy and the acceleration input a q can be chosen as before according to 


(8.191. 


In reality, however, such “acceleration actuators” are not available to us and 
we must be content with the ability to produce a generalized force (torque) iq 


at each joint i. Comparing equations (8.251 and (8.261 we see that the torque 
u and the acceleration a q of the manipulator are related by 


M 1 {u(t) - C(q, q)q-g(q)} = 


(8.27) 


By the invertibility of the inertia matrix we may solve for the input torque u(t) 
as 


u = M(q)a q + C(q, q)q + g(q) 


(8.28) 


which is the same as the previously derived expression (18. 17 ) . Thus the inverse 


dynamics can be viewed as an input transformation which transforms the prob- 
lem from one of choosing torque input commands, which is difficult, to one of 
choosing acceleration input commands, which is easy. 

Note that the implementation of this control scheme requires the computa- 
tion at each sample instant of the inertia matrix M ( q ) and the vector of Coriolis, 


centrifugal, and gravitational. Unlike the computed torque scheme (7.531, how- 
ever, the inverse dynamics must be computed on-line. In other words, as a 
feedback control law, it cannot be precomputed off-line and stored as can the 
computed torque (7.541. An important issue therefore in the control system 
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implementation is the design of the computer architecture for the above com- 
putations. As processing power continues to increase the computational issues 
of real-time implementation become less important. An attractive method to 
implement this scheme is to use a dedicated hardware interface, such as a DSP 
chip, to perform the required computations in real time. Such a scheme is 
shown in Figure [8d] 


LINEARIZED SYSTEM 



Fig. 8.1 Inner loop/outer control architecture. 


Figure 8.1 illustrates the notion of inner-loop/outer-loop control. By this we 


mean that the computation of the nonlinear control (8.171 is performed in an 


inner loop, perhaps with a dedicated hardwire interface, with the vectors q, q , 
and a q as its inputs and u as output. The outer loop in the system is then the 
computation of the additional input term a q . Note that the outer loop control 
a q is more in line with the notion of a feedback control in the usual sense of 
being error driven. The design of the outer loop feedback control is in theory 
greatly simplified since it is designed for the plant represented by the dotted 
lines in Figure |8.1[ which is now a linear or nearly linear system. 


8.3.1 Task Space Inverse Dynamics 

As an illustration of the importance of the inner loop/outer loop paradigm, we 
will show that tracking in task space can be achieved by modifying our choice 


of outer loop control q in (8.18) while leaving the inner loop control unchanged. 
Let X £ R 6 represent the end-effector pose using any minimal representation 
of S'O(S). Since X is a function of the joint variables q £ C we have 


X = J(q)q 
X = J(q)q + j{q)q. 


(8.29) 

(8.30) 
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where J = J a is the analytical Jacobian of section |4.8| Given the double 

integrator system, (8.18 1, in joint space we see that if a q is chosen as 

a q = J^ 1 ja.Y — (8.31) 

the result is a double integrator system in task space coordinates 

X = a x (8.32) 


Given a task space trajectory X d (t), satisfying the same smoothness and bound- 
edness assumptions as the joint space trajectory q d {t), we may choose ax as 


a x =X d + K P {X d -X) + K D {X d - X) 
so that the Cartesian space tracking error, X = X — X d , satisfies 

X + K d X + KpX = 0 . 


(8.33) 

(8.34) 


Therefore, a modification of the outer loop control achieves a linear and decou- 
pled system directly in the task space coordinates, without the need to compute 
a joint space trajectory and without the need to modify the nonlinear inner loop 
control. 

Note that we have used a minimal representation for the orientation of the 
end-effector in order to specify a trajectory I e R 6 . In general, if the end- 
effector coordinates are given in SE( 3), then the Jacobian J in the above for- 
mulation will be the geometric Jacobian J. In this case 


and the outer loop control 


a g = J 1 (q){(^ al'j - Aq)q} 


applied to (8.181 results in 

the system 

X = 

a x G M 3 

Cj = 

a u G R 3 

R = 

S(u>)R, R G 50(3), S G so(3) 


(8.35) 


(8.36) 


(8.37) 

(8.38) 

(8.39) 


Although, in this latter case, the dynamics have not been linearized to a double 
integrator, the outer loop terms a v and a u may still be used to defined control 
laws to track end-effector trajectories in SE( 3 ). 

In both cases we see that non-singularity of the Jacobian is necessary to 
implement the outer loop control. If the robot has more or fewer than six 
joints, then the Jacobians are not square. In this case, other schemes have been 
developed using, for example, the pseudoinverse in place of the inverse of the 
Jacobian. See [?] for details. 
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8.4 ROBUST AND ADAPTIVE MOTION CONTROL 

A drawback to the implementation of the inverse dynamics control methodol- 
ogy described in the previous section is the requirement that the parameters of 
the system be known exactly. If the parameters are not known precisely, for 
example, when the manipulator picks up an unknown load, then the ideal perfor- 
mance of the inverse dynamics controller is no longer guaranteed. This section 
is concerned with robust and adaptive motion control of manipulators. The 
goal of both robust and adaptive control to maintain performance in terms of 
stability, tracking error, or other specifications, despite parametric uncertainty, 
external disturbances, unmodeled dynamics, or other uncertainties present in 
the system. In distinguishing between robust control and adaptive control, we 
follow the commonly accepted notion that a robust controller is a fixed con- 
troller, static or dynamic, designed to satisfy performance specifications over a 
given range of uncertainties whereas an adaptive controller incorporates some 
sort of on-line parameter estimation. This distinction is important. For exam- 
ple, in a repetitive motion task the tracking errors produced by a fixed robust 
controller would tend to be repetitive as well whereas tracking errors produced 
by an adaptive controller might be expected to decrease over time as the plant 
and/or control parameters are updated based on runtime information. At the 
same time, adaptive controllers that perform well in the face of parametric un- 
certainty may not perform well in the face of other types of uncertainty such as 
external disturbances or unmodeled dynamics. An understanding of the trade- 
offs involved is therefore important in deciding whether to employ robust or 
adaptive control design methods in a given situation. 

Many of the fundamental theoretical problems in motion control of robot 
manipulators were solved during an intense period of research from about the 
mid-1980’s until the early-1990’s during which time researchers first began to 
exploit fundamental structural properties of manipulator dynamics such as feed- 
back linearizability, passivity, multiple time-scale behavior, and other properties 
that we discuss below. 

8.4.1 Robust Feedback Linearization 

The feedback linearization approach relies on exact cancellation of nonlinear- 
ities in the robot equations of motion. Its practical implementation requires 
consideration of various sources of uncertainties such as modeling errors, un- 
known loads, and computation errors. Let us return to the Euler-Lagrange 
equations of motion 


M (q)q + C{q , q)q + g(q) = u 


(8.40) 


and write the inverse dynamics control input u as 
u = M(q)a q + C(q , q)q + g(q) 


(8.41) 
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where the notation (•) represents the computed or nominal value of (•) and 
indicates that the theoretically exact feedback linearization cannot be achieved 
in practice due to the uncertainties in the system. The error or mismatch 
(•) = (•) — (•) is a measure of one’s knowledge of the system dynamics. 

If we now substitute (8.41) into (8.40) we obtain, after some algebra, 


q = a q + 


v(q,q,a q ) 


where 


r) = M 1 (Ma q + Cq + g) 
is called the Uncertainty. We note that 

M~ X M = M -1 M — I =: E 
and so we may decompose rj as 

V = Ea q + M~ 1 (Cq + g) 


(8.42) 


(8.43) 


(8.44) 


(8.45) 


We note that the system (8.421 is still nonlinear and coupled due to the uncer- 


tainty q(q, q , a q ). T hus we have no guarantee that the outer loop control given 
by Equation (8.19) will satisfy desired tracking performance specifications. In 


this chapter we discuss several design methods to modify the outer loop control 


(??) to guarantee global convergence of the tracking error for the system (8.42 1. 


8. 4. 1.1 Outer Loop Design via Lyapunov's Second Method There are several 
approaches to treat the robust feedback linearization problem outlined above. 
We will discuss only one method, namely the so-called theory of guaran- 
teed stability of uncertain systems, which is based on Lyapunov’s second 
method. In this approach we set the outer loop control a q as 

q = q d (t ) + K P (q d - q) + K D (q d - q) + Sa (8.46) 


In terms of the tracking error 


Q 


q - q d ' 

. q . 


1 

■a 

1 

•CJH 

1 


(8.47) 


we may write 


e = Ae + B{Sa + 77 } 


where 


A = 


0 I 

-K P -K d 



(8.48) 

(8.49) 


Thus the double integrator is first stabilized by the linear feedback, —Kpe — 
Kp>e, and Sa is an additional control input that must be designed to overcome 
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the potentially destabilizing effect of the uncertainty 77. The basic idea is to 
compute a time-varying scalar bound, p(e,f) > 0, on the uncertainty 77, i.e., 

\\r)\\ < p(e,t) (8.50) 


and design the additional input term 5a to guarantee ultimate boundedness of 
the state trajectory x(t) in (8.48 1. 

Returning to our expression for the uncertainty 


77 = Eq + M-\Cq + g) 

= E5a + E(q d - K P e- K D e) + M-^Cq + g) 


(8.51) 

(8.52) 


we assume a bound of the form 


IMI < a||<5a|| + 7i||e|| +7 2 ||e|| 2 +73 (8.53) 

where a = ||P|| = \\M~ l M — I\\ and 7 * are nonnegative constants. Assuming 
for the moment that | [<5a| < p(e,t), which must then be checked a posteriori, 
we have 

IMI < ap{e,t)+ 71 1 1 e 1 1 +7 2 ||e|| 2 + 73 =: p(e,t) (8.54) 

which defines p as 


P(e,t) 


1 

1 — a 


e\\ + 7 2 ||e 


73) 


(8.55) 


Since Kp and Kp> are chosen in so that A in (8.48) 
choose Q > 0 and let P > 0 be the unique symmetric 
satisfying the Lyapunov equation, 


is a Hurwitz matrix, we 
positive definite matrix 


A t P+PA = -Q. 


(8.56) 


Defining the control 5a according to 




5a = 


B T Pe 

\B T Pe\\ 


0 


if ||R T Pe ||^0 
if ||P T Pe||=0 


(8.57) 


it follows that the Lyapunov function V = e T Pe satisfies V < 0 along solution 
trajectories of the system (8.481. To show this result, we compute 

V = ~e T Qe + 2e T PB{5a + 77 } (8.58) 


For simplicity, set w = B T Pe and consider the second term, w T {8a + 77 } in the 
above expression. If w = 0 this term vanishes and for w / 0, we have 


5a = —p 


w 


IM 


(8.59) 
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and (8.581 becomes, using the Cauchy-Schwartz inequality, 


w 


w 1 (— p-r — n+v) < -P\\M\ + Ik 


\w\ 


= IMK-p- 


< 0 


(8.60) 

(8.61) 


since ||??|| < p and hence 

V < -e T Qe (8.62) 

and the result follows. Note that ||<Ja|| < p as required. 

Since the above control term Sa is discontinuous on the manifold defined 
by B t Pe = 0, solution trajectories on this manifold are not well defined in 
the usual sense. One may define solutions in a generalized sense, the so-called 
Filipov solutions [?]. A detailed treatment of discontinuous control systems 
is beyond the scope of this text. In practice, the discontinuity in the control 
results in the phenomenon of chattering , as the control switches rapidly across 
the manifold B T Pe = 0. One may implement a continuous approximation to 
the discontinuous control as 


Sa = 


_ 0 ( e t) B 1 Pe 
P[eit ’\\B T Pe\\ 

_P^l B Tp e 


if ||B T Pe||>e 
if ||S T Pe||<e 


(8.63) 


In this case, since the control signal (8.63 a solution to the system (8.48) exists 
and is uniformly ultimately bounded (u.u.b). Ssee Appendix C for the definition 
of uniform ultimate boundedness. 


Theorem 1 The origin of the system ( S.fSP is u.u.b. with respect to the set 
S, defined below, using the continuous control law (8.63). 


Proof: As before, choose 17(e) = e T Pe and compute 
V = —e T Qe + 2w T (Sa + r]) 


w 


< — e 1 Qe + Zw 1 {5a + pj — rr) 


(8.64) 

(8.65) 


with ||u>|| = ||.B T Pe|| as above. 

For 1 1 to 1 1 > e the argument proceeds as above and V < 0. For ||w|| < e the 
second term above becomes 

„ p W . 

2 W T { W+ Pj— rr) 

e M 


= — 2 -|| w || 2 


2p\\i 


This expression attains a maximum value of eR when ||tu|| = Thus we have 


V = ~e T Qe + < 0 


( 8 . 66 ) 
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provided 


—e T Qe > 

(8.67) 

Using the relationship 


A m in(Q) e 2 < e T Qe < A maa: ((3)||e|| 2 

(8.68) 


where A min(Q), ^max(Q) denote the minimum and maximum eigenvalues, re- 
spectively, of the matrix Q, we have that V < 0 if 


Amin(Q)||e|] 2 > e— 


(8.69) 


or, equivalently 


> 


ep 


2A min (Q) 


=: 5 


(8.70) 


Let Sg denote the smallest level set of V containing B(S), the ball of radius 
S and let B r denote the smallest ball containing Sg- Then all solutions of the 
closed loop system are u.u.b. with respect to S := B r . The situation is shown 
in Figure 8.2 All trajectories will eventually enter the ball, B r ] in fact, all 


trajectories will reach the boundary of Sg since V is negative definite outside 
of Sg. 


Fig. 8.2 Uniform Ultimate Boundedness Set 


8.4.2 Passivity Based Robust Control 

In this section we derive an alternative robust control algorithm which exploits 
the passivity and linearity in the parameters of the rigid robot dynamics. This 
methods are qualitatively different from the previous methods which were based 
on feedback linearization. In the passivity based approach we modify the inner 
loop control as 

u = M(q)a + C(q , q)v + g{q) — Kr. (8-71) 

where v, a, and r are given as 

v = q d - Aq 
a = v = q d - Aq 
r = q d — v = q + Aq 


with I\ , A diagonal matrices of positive gains. In terms of the linear parametriza- 
tion of the robot dynamics, the control (8.71) becomes 


u = Y(q,q, a,v)9 — Kr 


(8.72) 
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and the combination of (8.711 with (8.401 yields 

M(q)r + C(q, q)r + Kr = Y(0 - 8 0 ). (8.73) 

Note that, unlike the inverse dynamics control, the modified inner loop control 
( 8.40 1 does not achieve a linear, decoupled system, even in the known parameter 
case 0 = 0. 


In the robust passivity based approach of [?], the term 8 in (8.721 is chosen 


as 


0 = 8 0 + u (8.74) 

where 0q is a fixed nominal parameter vector and u is an additional control 
term. The system ( |8.73| ) then becomes 

M(q)r + C{q, q)r + Kr = Y(a,v,q,q)(0 + u ) (8.75) 

where 8 = 9q — 8 is a constant vector and represents the parametric uncertainty 
in the system. If the uncertainty can be bounded by finding a nonnegative 
constant, p > 0, such that 

||fl|| = ||0o-0||<P, (8-76) 


then the additional term u can be designed according to the expression, 

^ p w r k\ ; if ||yTr||>e 


The Lyapunov function 


(8.77) 


— ^ Y T r ; if ||y T r|| < 


V = -r T M(q)r + q T AKq 


(8.78) 


is used to show uniform ultimate boundedness of the tracking error. Calculating 
V yields 


V = r T Mr + -r T Mr + 2q T KI\q 

1 


= — r 1 Kr + 2 q 1 AKq + -r 1 (M — 2 C)r + r 1 Y (9 + u) 

Using the passivity property and the definition of r, this reduces to 
V = ~q T A T KAq-q T Kq + r T Y(9 + u) 
Defining w = Y T r and 

_ [ A t KA 0 
Q ~ 0 AK 


(8.79) 

(8.80) 


(8.81) 

(8.82) 


ROBUST AND ADAPTIVE MOTION CONTROL 277 


and mimicking the argument in the previous section, we 
V = —e T Qe + w T (u + 9) 

T /~\ T ( W . 

= —e Qe + w (u + p - — -) 
ky 


have 


(8.83) 

(8.84) 


Uniform ultimate boundedness of the tracking error follows with the control u 
from (8.77) exactly as in the proof of Theorem [l] 


Comparing this approach with the approach in the section (8.4.1), we see 


that finding a constant bound p for the constant vector 9 is much simpler 


than finding a time-varying bound for 77 in (8.43 1 . The bound p in this case 


depends only on the inertia parameters of the manipulator, while p{x, t ) in 


(8.50 1 depends on the manipulator state vector, the reference trajectory and, 


in addition, requires some assumptions on the estimated inertia matrix M{q). 


8.4.3 Passivity Based Adaptive Control 


In the adaptive approach the vector 9 in (8.721 is taken to be a time- varying 


estimate of the true parameter vector 9. Combining the control law (8.71 ) with 


(8.401 yields 


M(q)r + C(q , q)r + Kr = Y9 


(8.85) 


The parameter estimate 9 may be computed using standard methods such as 
gradient or least squares. For example, using the gradient update law 


e=-V~ L Y l (q,q,a,v)r 


together with the Lyapunov function 

1 
2 


V = -r T M(q)r 


q T AKq 


-9 t T0 


( 8 . 86 ) 


(8.87) 


results in global convergence of the tracking errors to zero and boundedness of 
the parameter estimates. 

To show this, we first note an important difference between the adaptive 
control approach and the robust control approach from the previous section. In 
the robust approach the state vector of the system is (q,q) T . In the a daptive 


control approach, the fact that 9 satisfies the differential equation ( 8.86 


means 


that the complete state vector now includes 9 and the state equations are given 
by the couple system ( 8.85 )-( 8.86). For this r eason we included the positive 


definite term \9 t T9 in the Lyapunov function (8.87). 


If we now compute V along trajectories of the system (8.85), we obtain 


V = -q 1 A 1 KAq - q 1 Kq + 9 1 {F 6 » + Y 1 r} 


( 8 . 88 ) 


4 Note that 9 = 9 since the parameter vector 9 is constant 
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Substituting the expression for 6 from gradient update law (8.86 1 into (8.881 
yields 


V = —q 1 ‘K 1 'KAq - q T I<q = -e 1 Qe < 0 


(8.89) 


where e and Q are defined as before, showing that the closed loop system is 
stable in the sense of Lyapunov. 

Remark 8.1 Note that we have claimed only that the Lyapunov function is 
negative semi- definite, not negative definite since V does not contain any terms 
that are negative definite in 9. In fact, this situation is common in most gra- 
dient based adaptive control schemes and is a fundamental reason for several 
difficulties that arise in adaptive control such as lack of robustness to external 
disturbances and lack of parameter convergence. A detailed discussion of these 
and other problems in adaptive control is outside the scope of this text. 

Returning to the problem at hand, although we conclude only stability in the 


sense of Lyapunov for the closed loop system (8.851 (8.861, further analysis will 


allow to reach stronger conclusions. First, note that since V is nonincreasing, 
the value of V(t) can be no greater than its value at t = 0. Since V consists of 
a sum of nonnegative terms, this means that each of the terms r, q, and'theta 
are bounded as functions of time, t. Thus we immediately conclude that the 
parameter estimation error is bounded. 

With regard to the tracking error, q, ~q, we also note that, V is not simply 
negative but also quadratic in the error vector eft). Integrating both sides of 


Equation (8.89) gives 


V(£) — V(0) = — f e t (a)Qe(a)do < oo 

Jo 


(8.90) 


As a consequence, V is a so-called square integrable function. Such functions, 
under some mild additional restrictions must tend to zero as t — > oo. Specifi- 
cally, we may appeal to the following lemma[?] 

Lemma 8.1 Suppose f : R i— > ffi. is a square integrable function and further 
suppose that its derivative f is bounded. Then f(t) — > 0 as t — > oo. 

We note that, since both r = q + A q and q have already been shown to be 
bounded, it follows that q is also bounded. Therefore we have that q is square 
integrable and its derivative is bounded. Hence the tracking error q — > 0 as 
t — > oo. 

To show that the velocity tracking error also converges to zero, one must 


appeal to the equations of motion (8.851. From Equation (8.85) one may argue 


(Problem ??) that the acceleration q is bounded. The result will follow assuming 
that the reference acceleration q d (t ) is also bounded (Problem ??). 
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Problems 


8-1 Form the Lagrangian for an n-link manipulator with joint flexibility using 
(??)-(??). From this derive the dynamic equations of motion (??)-(??). 

8-2 Complete the proof of stability of PD-control for the flexible joint robot 
without gravity terms using (??) and LaSalle’s Theorem. 

8-3 Suppose that the PD control law (??) is implemented using the link vari- 
ables, that is, 

u = Kpfji - K n q { . 

What can you say now about stability of the system? Note: This is a 
difficult problem. Try to prove the following conjecture: Suppose that 
B 2 = 0 in (??). Then with the above PD control law, the system is 
unstable. [Hint: Use Lyapunov’s First Method, that is, show that the 
equilibrium is unstable for the linearized system.] 

8-4 Using the control law (??) for the system (??)-(??), what is the steady 
state error if the gravity terms are present? 

8-5 Simulate an inverse dynamics control law for a two- link elbow manipulator 
whose equations of motion were derived in Chapter ??. Investigate what 
happens if there are bounds on the maximum available input torque. 

8-6 For the system of Problem 8-5 what happens to the response of the system 
if the coriolis and centrifugal terms are dropped from the inverse dynamics 
control law in order to facilitate computation? What happens if incorrect 
values are used for the link masses? Investigate via computer simulation. 

8-7 Add an outer loop correction term Av to the control law of Problem 8-6 
to overcome the effects of uncertainty. Base your design on the Second 
Method of Lyapunov as in Section ??. 

8-8 Consider the coupled nonlinear system 

yi + 3 2 / 12/2 + 2/2 = «i + 2 / 2^2 
2/2 + cos 2 / 12/2 + 3 ( 2/1 - 2 / 2 ) = U 2 - 3 (cos 2 /i) 2 2 / 2 «i 

where u\,u 2 are the inputs and 2/1 , 2/2 are the outputs. 

a) What is the dimension of the state space? 

b) Choose state variables and write the system as a system of first order 

differential equations in state space. 

c) Find an inverse dynamics control so that the closed loop system is 

linear and decoupled, with each subsystem having natural frequency 
10 radians and damping ratio 1/2. 





9 

FORCE CONTROL 


9.1 INTRODUCTION 

In previous chapters we considered the problem of tracking motion trajectories 
using a variety of basic and advanced control methods. Such position control 
schemes are adequate for tasks such as materials transfer or spot welding where 
the manipulator is not interacting significantly with objects in the workplace 
(hereafter referred to as the environment). However, tasks such as assembly, 
grinding, and deburring, which involve extensive contact with the environment, 
are often better handled by controlling the /orce^j of interaction between the 
manipulator and the environment rather than simply controlling the position 
of the end-effector. For example, consider an application where the manipula- 
tor is required to wash a window, or to write with a felt tip marker. In both 
cases a pure position control scheme is unlikely to work. Slight deviations of 
the end-effector from a planned trajectory would cause the manipulator either 
to lose contact with the surface or to press too strongly on the surface. For 
a highly rigid structure such as a robot, a slight position error could lead to 
extremely large forces of interaction with disastrous consequences (broken win- 
dow, smashed pen, damaged end-effector, etc.). The above applications are 
typical in that they involve both force control and trajectory control. In the 
window washing application, for example, one clearly needs to control the forces 
normal to the plane of the window and position in the plane of the window. 


1 Hereafter we use force to mean force and/or torque and position to mean position and/or 
orientation. 
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A force control strategy is one that modifies position trajectories based on 
the sensed force. There are three main types of sensors for force feedback, wrist 
force sensors, joint torque sensors, and tactile or hand sensors. A wrist force 
sensor such as that shown in Figure |9.1| usually consists of an array of strain 



Fig. 9.1 A Wrist Force Sensor. 

gauges and can delineate the three components of the vector force along the 
three axes of the sensor coordinate frame, and the three components of the 
torque about these axes. A joint torque sensor consists of strain gauges located 
on the actuator shaft. Tactile sensors are usually located on the fingers of the 
gripper and are useful for sensing gripping force and for shape detection. For 
the purposes of controlling the end-effector/environment interactions, the six- 
axis wrist sensor usually gives the best results and we shall henceforth assume 
that the manipulator is equipped with such a device. 


9.2 COORDINATE FRAMES AND CONSTRAINTS 

Force control tasks can be thought of in terms of constraints imposed by 
the robot/environment interaction. A manipulator moving through free space 
within its workspace is unconstrained in motion and can exert no forces since 
there is no source of reaction force from the environment. A wrist force sensor 
in such a case would record only the inertial forces due to any acceleration of 
the end-effector. As soon as the manipulator comes in contact with the en- 
vironment, say a rigid surface as shown in Figure |9.2[ one or more degrees of 
freedom in motion may be lost since the manipulator cannot move through the 
environment surface. At the same time the manipulator can exert forces against 
the environment. 

In order to describe the robot/environment interaction, let V = (v,cu) rep- 
resent the instantaneous linear and angular velocity of the end-effector and let 
F = (/, n) represent the instantaneous force and moment. The vectors V and F 
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Fig. 9.2 Robot End-Effector in contact with a Rigid Surface 


are each elements of six dimensional vector spaces, which we denote by M. and 
T , the motion and force spaces, respectively. The vectors V and F are called 
Twists and Wrenches in more advanced texts [?] although we will continue to 
refer to them simply as velocity and force for simplicity. If e\, . . . , ee is a basis 
for the vector space so(3), and /i, . . . , fe is a basis for so* (3), we say that these 
basis vectors are reciprocal provided 

e-ifj = 0 if i ± j 
dfj = 1 if i = j 

We can define the product of V € so(3) and F € so* (3) in the usual way as 

V ■ F = V T F = v T f + u T n (9.1) 

The advantage of using reciprocal basis vectors is that the product V T F is 
then invariant with respect to a linear change of basis from one reciprocal 
coordinate system to another. We note that expressions such as V^V 2 or F-[ F 2 
for vectors V*, T) belonging to so(3) and so*(3), respectively, are not necessarily 
well defined. For example, the expression 


V?V 2 =v1v 2 + u>1w)2 (9.2) 

is not invariant with respect to either choice of units or basis vectors in so(3). It 
is possible to define inner product like operations, i.e. symmetric, bilinear forms 
on so(3) and so* (3), which have the necessary invariance properties. These are 
the so-called Klein Form, KL(V \,V 2 ), and Killing Form, KI{ V\,V 2 ), defined 
according to 


= Vi U> 2 + Ulf V 2 
T 

= UJ 1 U ) 2 


KL(V \,V 2 ) 
KI(V u V 2 ) 


(9.3) 

(9.4) 
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However, a detailed discussion of these concepts is beyond the scope of this text. 
As the reader may suspect, the need for a careful treatment of these concepts 
is related to the geometry of 50(3) as we have seen before in other contexts. 
Example 9.1 /?/ Suppose that 

Vi = (1) 1, 1, 2, 2, 2) t 

y 2 = (2, 2, 2, —1, —1, — 1) T 


where the linear velocity is in meters/sec and angular velocity is in radians/sec. 
The clearly , ViV 2 = 0 and so one could infer that V\ and V 2 are orthogonal 
vectors in so(3). However, suppose now that the linear velocity is represented 
in units of centimeter s/sec. Then 


Vl = (1 x 10 2 , 1 x 10 2 , 1 x 10 2 , 2, 2, 2 ) t 
V 2 = (2 x 10 2 , 2 x 10 2 , 2 x 10 2 , — 1, — 1, — 1) T 


and clearly ViV 2 ^ 0. Thus, usual notion of orthogonality is not meaningful 
in so(3). It is easy to show that the equality KL(yi,V 2 ) = 0 (respectively, 
KI(\ i,V 2 ) = 0) is independent of the units or the basis chosen to represent 
Vi and V 2 . For example, the condition KI(y\,V 2 ) = 0 means that the axes of 
rotation defining u)\ and lo 2 are orthogonal, o 

We shall see in specific cases below that the reciprocity relation (9.1 1 may 
be used to design reference inputs to execute motion and force control tasks. 


9.2.1 Natural and Artificial Constraints 


In this section we discuss so-called Natural Constraints which are defined using 


the reciprocity condition (9.1 1. We then discuss the notion of Artificial Con- 


straints , which are used to define reference inputs for motion and force control 
tasks. 

We begin by defining a so-called Compliance Frame o c x c y c z c (also called a 
Constraint Frame ) in which the task to be performed is easily described. For 
example in the window washing application we can define a frame at the tool 
with the z c -axis along the surface normal direction. The task specification would 
then be expressed in terms of maintaining a constant force in the z c direction 
while following a prescribed trajectory in the x c — y c plane. Such a position 
constraint in the z c direction arising from the presence of a rigid surface is a 
natural constraint. The force that the robot exerts against the rigid surface 
in the z c direction, on the other hand, is not constrained by the environment. 
A desired force in the z c direction would then be considered as an artificial 
constraint that must be maintained by the control system. 

Figure [973] shows a typical task, that of inserting a peg into a hole. With 
respect to a compliance frame o c x c y c z c as shown at the end of the peg, we may 
take the the standard orthonormal basis in 3? 6 for both so(3) and so* (3), in 
which case 


V T F = V X f X + Vyfy + V Z f Z + U) x n x + LVyUy + U Z 71 


(9.5) 
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If we assume that the walls of the hole and the peg are perfectly rigid and there 
is no friction, it is easy to see that 


v x = 0 v y = 0 f z = 0 
lo x = 0 u) y = 0 n z = 0 


(9.6) 


and thus the reciprocity condition V T F = 0 is satisfied, 
ships (9.6 1 are termed Natural Constraints. 


These relation- 



Flg. 9.3 Inserting a peg into a hole. 


Examining Equation (9.5 1 we see that the variables 


fx fy T z Ll x Tly U) z 


(9.7) 


are unconstrained by the environment, i.e. given the natural constraints ( |9.6| ), 
the reciprocity condition V T F = 0 holds for all values of the above variables 
(9.7). We may therefore assign reference values, called Artificial Constraints , 


for these variables that may then be enforced by the control system to carry out 
the task at hand. For example, in the peg-in-hole task we may define artificial 
constraints as 


fx =0 fy = 0 V z = V d 
n x = 0 n y = 0 lo z = 0 


(9.8) 


where v d is the desired speed of insertion of the peg in the z-direction. 

Figures [974] and pT77| show natural and artificial constraints for two additional 
tasks, that of turning a crank and and turning a screw, respectively. 


9.3 NETWORK MODELS AND IMPEDANCE 

The reciprocity condition V T F = 0 means that the forces of constraint do no 
work in directions compatible with motion constraints and holds under the ideal 
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Fig. 9.4 Turning a crank 


2 




Fig. 9.5 Turning a screw. 


conditions of no friction and perfect rigidity of both the robot and environment. 

In practice, compliance and friction in the robot/environment interface will 
alter the strict separation between motion constraints and force constraints. For 
example, consider the situation in Figure [976] Since the environment deforms in 
response to a force, there is clearly both motion and force in the same direction, 
i.e. normal to the surface. Thus the product V(t)F(t) along this direction will 
not be zero. Let k represent the stiffness of the surface so that / = kx. Then 

[ V(u)F(u)du= [ x(u)kx(u)du = k f ~-kx 2 (u)du = -k(x 2 (t) — x 2 ( 0))(9.9) 
J 0 Jo Jo du 2 2 

is the change of the potential energy. The environment stiffness, k, determines 
the amount of force needed to produce a given motion. The higher the value of 
k the more the environment “impedes” the motion of the end-effector. 

In this section we introduce the notion of Mechanical Impedance which cap- 
tures the relation between force and motion. We introduce so-called Network 
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Fig. 9.6 Compliant Environment 


Models , which are particularly useful for modeling the robot/environment inter- 
action. We model the robot and environment as One Port Networks as shown 
in Figure |9?7| The dynamics of the robot and environment, respectively, deter- 
mine the relation between the Port Variables, V r , F r , and V e , F e , respectively. 
F r , F e are known as Effort or Across variables while V r , V e are known as Flow 
or Through variables. In a mechanical system, such as a robot, force and veloc- 
ity are the effort and flow variables while in an electrical system, voltage and 
current are the effort and flow variables, respectively. With this description, 



Fig. 9.7 One-Port Networks 

the “product” of the port variables, V T F, represents instantaneous power and 
the integral of this product 

V T (a)F(a)da 

is the Energy dissipated by the Network over the time interval [0, t] . 

The robot and the environment are then coupled through their interaction 
ports, as shown in Figure [978] which describes the energy exchange between the 
robot and the environment. 
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Fig. 9.8 Robot /Environment Interaction 


9.3.1 Impedance Operators 


The relationship between the effort and flow variables may be described in terms 
of an Impedance Operator. For linear, time invariant systems, we may utilize 
the s-domain or Laplace domain to define the Impedance. 


Definition 9.1 Given the one-port network 9.7 the Impedance, Z(s) is defined 
as the ratio of the Laplace Transform of the effort to the Laplace Transform of 
the flow, i.e. 


Z(s) = 


F(s) 

V(s) 


(9.10) 


Example 9.2 Suppose a mass-spring-damper system is described by the dif- 
ferential equation 


Mx + Bx + Kx = F (9.11) 

Taking Laplace Transforms of both sides (assuming zero initial conditions) it 
follows that 

Z(s) = F(s)/V(s) = Ms + B + K/s (9.12) 

o 

9.3.2 Classification of Impedance Operators 

Definition 9.2 An impedance Z(s) in the Laplace variable s is said to be 

1. Inertial if and only if \Z(0)\ = 0 

2. Resistive if and only if \Z(0) \ = B for some constant 0 < B < oo 

3. Capacitive if and only if \Z( 0)| = oo 

Thus we classify impedance operators based on their low frequency or DC-gain, 
which will prove useful in the steady state analysis to follow. 
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Example 9.3 Figure \T9 \ shows examples of environment types. Figure \9T^( a) 
shows a mass on a frictionless surface. The impedance is Z(s) = Ms, which is 
inertial. Figure \9^( b) shows a mass moving in a viscous medium with resistance 
B. Then Z(s ) = Ms + B, which is resistive. Figure \9T)( c) shows a linear spring 
with stiffness K. Then Z{s) = K/s, which is capacitive, o 


if 



I d) Tnprhft L 



I b) Resisti'4 \ c) c* pacific 


\ 


Fig. 9.9 Inertial, Resistive, and Capacitive Environment Examples 


9.3.3 Thevenin and Norton Equivalents 

In linear circuit theory it is common to use so-called Thevenin and Norton 
equivalent circuits for analysis and design. It is easy to show that any one-port 
network consisting of passive elements (resistors, capacitors, inductors) and 
active current or voltage sources can be represented either as an impedance, 
Z(s), in series with an effort source (Thevenin Equivalent) or as an impedance, 
Z(s), in parallel with a flow source (Norton Equivalent). The independent 



F 





Fig. 9.10 Thevenin and Norton Equivalent Networks 
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sources, F s and V s may be used to represent reference signal generators for 
force and velocity, respectively, or they may represent external disturbances. 


9.4 TASK SPACE DYNAMICS AND CONTROL 

Since a manipulator task specification, such as grasping an object, or inserting 
a peg into a hole, is typically given relative to the end- effector frame, it is 
natural to derive the control algorithm directly in the task space rather than 
joint space coordinates. 


9.4.1 Static Force/Torque Relationships 

Interaction of the manipulator with the environment will produce forces and 
moments at the end-effector or tool. Let F = (F x , F y , F z , n x , n y , n z ) T repre- 
sent the vector of forces and torques at the end-effector, expressed in the tool 
frame. Thus F x ,F y , F z are the components of the force at the end-effector, and 
n x ,n y ,n z are the components of the torque at the end-effector. 

Let r denote the vector of joint torques, and let SX represent a virtual end- 
effector displacement caused by the force F. Finally, let 5q represent the cor- 
responding virtual joint displacement. These virtual displacements are related 
through the manipulator Jacobian J(q) according to 


SX = J(q)Sq. 

The virtual work Sw of the system is 

Sw = F t SX — r T 5q. 


Substituting (9.13 1 ) into (9.141 yields 

Sw = ( F T J—T T )5q 


(9.13) 


(9.14) 


(9.15) 


which is equal to zero if the manipulator is in equilibrium. Since the generalized 
coordinates q are independent we have the equality 

T = J{q) T F. (9.16) 


In other words the end-effector forces are related to the joint torques by the 


transpose of the manipulator Jacobian according to (9.16). 


Example 9.4 Consider the two-link planar manipulator of Figure \9.11\ with 
a force F = ( F x ,F y ) T applied at the end of link two as shown. The Jacobian 
of this manipulator is given by Equation (4-86). The resulting joint torques 
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Fig. 9.11 Two-link planar robot. 


r = (ti , T 2 ) are then given as 


F x 

Fy 


n 


— OiSi — (I2S12 

aiCi + a 2 Ci 2 

0 

0 

0 

1 ' 

_ t 2 _ 


-a 2 si 2 

LL2C12 

0 

0 

0 

1 


o 

9.4.2 Task Space Dynamics 

When the manipulator is in contact with the environment, the dynamic equa- 
tions of Chapter [h] must be modified to include the reaction torque J T F e cor- 
responding to the end-effector force F e . Thus the equations of motion of the 
manipulator in joint space are given by 

M(q)q + C(q,q)q + g(q) + J T (q)F e = u (9.18) 

Let us consider a modified inverse dynamics control law of the form 

u = M(q)a q + C(q , q)q + g(q) + J T (q)a f (9.19) 

where a q and a/ are outer loop controls with units of acceleration and force, 
respectively. Using the relationship between joint space and task space variables 
derived in Chapter [8] 


a. 


x 


J(q)q + J{q)q 

J(q)a q + j(q)q 


(9.20) 

(9.21) 
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we substitute ( 9.19 )-( 9.21) into (9.18) to obtain 


x = a x + W(q)(F e - af ) 


(9.22) 


where W(q) = J(q)M~ 1 (q)J T (q) is called the Mobility Tensor. There is often 
a conceptual advantage to separating the position and force control terms by 
assuming that a x is a function only of position and velocity and ay is a function 
only of force [?]. However, for simplicity, we shall take a/ = F e to cancel the 
environment force, F e and thus recover the task space double integrator system 


x = a 


X 


(9.23) 


and we will assume that any additional force feedback terms are included in the 
outer loop term a x . This entails no loss of generality as long as the Jacobian 
(hence W(q )) is invertible. This will become clear later in this chapter. 


9.4.3 Impedance Control 


In this section we discuss the notion of Impedance Control. We begin with an 
example that illustrates in a simple way the effect of force feedback 
Example 9.5 Consider the one- dimensional system in Figure 9.12 consisting 


V 


^ X 




Fig. 9.12 One Dimensional System 

of a mass, M, on a frictionless surface subject to an environmental force F and 
control input u. The equation of motion of the system is 

Mx = u — F (9.24) 

With u = 0, the object “appears to the environment ” as a pure inertia with mass 
M. Suppose the control input u is chosen as a force feedback term u = — mF . 
Then the closed loop system is 

M 

1 + m 


Mx = —(1 + m)F 


x = —F 


(9.25) 
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Hence, the object now appears to the environment as an inertia with mass 
^ ■ Thus the force feedback has the effect of changing the apparent inertia 

of the system, o 

The idea behind Impedance Control is to regulate the mechanical impedance, 
i.e., the apparent inertia, damping, and stiffness, through force feedback as in 
the above example. For example, in a grinding operation, it may be useful 
to reduce the apparent stiffness of the end-effector normal to the part so that 
excessively large normal forces are avoided. 

We may formulate Impedance Control within our standard inner loop/outer 
loop control architecture by specifying the outer loop term, a x , in Equation (9.23 ). 
Let x d (t) be a reference trajectory defined in task space coordinates and let M d , 
Bd, Kd, be 6 x 6 matrices specifying desired inertia, damping, and stiffness, re- 
spectively. Let e(t) = x(t) — x d (t) be the tracking error in task space and set 


x d M d 1 (B d e + K d e -I- F) 


(9.26) 


where F is the measured environmental force, 
yields the closed loop system 


Substituting (9.261 into (9.23 1 


M d e + B d e + K d e = -F 


(9.27) 


which results in desired impedance properties of the end-effector. Note that 
for F = 0 tracking of the reference trajectory, x d (t), is achieved, whereas for 
nonzero environmental force, tracking is not necessarily achieved. We will ad- 
dress this difficulty in the next section. 


9.4.4 Hybrid impedance Control 


In this section we introduce the notion of Hybrid Impedance Control following 
the treatment of [?]. We again take as our starting point the linear, decou- 
pled system (9.231. The impedance control formulation in the previous section 
is independent of the environment dynamics. It is reasonable to expect that 
stronger results may be obtained by incorporating a model of the environment 
dynamics into the design. For example, we will illustrate below how one may 
regulate both position and impedance simultaneously which is not possible with 
the pure impedance control law (9.26). 

We consider a one-dimensional system representing one component of the 
outer loop system (9.23) 

x.i = a Xi (9.28) 


and we henceforth drop the subscript, i, for simplicity. We assume that the 
impedance of the environment in this direction, Z e is fixed and known, a pri- 
ori. The impedance of the robot, Z ri is of course determined by the control 
input. The Hybrid Impedance Control design proceeds as follows based on the 
classification of the environment impedance into inertial, resistive, or capacitive 
impedances: 
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1. If the environment impedance, Z e (s), is capacitive, use a Norton network 
representation. Otherwise, use a Thevenin network representatiorj^] 

2. Represent the robot impedance as the Dual to the environment impedance. 
Thevenin and Norton networks are considered dual to one another. 


3. In the case that the environment impedance is capacitive we have the 
robot/environment interconnection as shown in Figure 9.13 where the 



Fig. 9.13 Capacitive Environment Case 


environment one-port is the Norton network and the robot on-port is the 
Thevenin network. Suppose that V s = 0, i.e. there are no environmental 
disturbances, and that F s represents a reference force. From the circuit 
diagram it is straightforward to show that 


F = Z e (s) 

F s Z e (s) + Z r (s) 


(9.29) 


Then the steady state force error, e ss , to a step reference force, F s = F- 
is given by the Final Value Theorem as 


-Zr(0) _ n 

z r ( 0) + Z e ( 0) 


(9.30) 


since Z e ( 0) = oo (capacitive environment) and Z r ^ 0 (non-capacitive 
robot). 

The implications of the above calculation are that we can track a constant 
force reference value, while simultaneously specifying a given impedance, 
Z r , for the robot. 


In order to realize this result we need to design outer loop control term a x 
in (9.281 using only position, velocity, and force feedback. This imposes a 
practical limitation on the the achievable robot impedance functions, Z r . 


2 In fact, for a resistive environment, either representation may be used 
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Suppose Z r 1 has relative degree one. This means that 

Z r (s) = M c s + Z rem (s) (9.31) 

where Z rem (s ) is a proper rational function. If we now choose 

&X = ~~ TT ZremX H (F s — F) (9.32) 

M c m c 

Substituting this into the double integrator system x = a x yields 

Z r (s) x = F S -F (9.33) 


Thus we have shown that, for a capacitive environment, force feedback can 
be used to regulate contact force and specify a desired robot impedance. 

4. In the case that the environment impedance is inertial we have the robot/environment 


interconnection as shown in Figure 9.14 where the environment one-port 



Fig. 9.14 Inertial Environment Case 


is a Thevenin network and the robot on-port is a Norton network. Sup- 
pose that F s = 0, and that V s represents a reference velocity. From the 
circuit diagram it is straightforward to show that 


V = Z r (s) 

V s Z e {s) + Z r (s) 


(9.34) 


Then the steady state force error, e ss , to a step reference velocity com- 
mand, V s = is given by the Final Value Theorem as 


~Ze( 0) _ Q 

Z r ( 0) + Z e ( 0) 


(9.35) 


since Z e ( 0) = 0 (inertial environment) and Z r yf 0 (non-inertial robot). 
To achieve this non-inertia robot impedance we take, as before, 


Z r (s) — M c s + Z rem (s) 


(9.36) 
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and set 

Lix = x d + j^jr Z rem (x d — x) + (9.37) 

Then, substituting this into the double integrator equation, x = a x , yields 

Z r (s){x d -x) =F (9.38) 

Thus we have shown that, for a capacitive environment, position control 
can be used to regulate a motion reference and specify a desired robot 
impedance. 
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Problems 


9-1 Given the two-link planar manipulator of Figure |9.11 find the joint torques 
T\ and r 2 corresponding to the end-effector force vector (— 1, — 1) T . 

9-2 Consider the two-link planar manipulator with remotely driven links shown 


in Figure 9.15 Find an expression for the motor torques needed to bal- 



Fig. 9.15 Two- link manipulator with remotely driven link. 

ance a force F at the end-effector. Assume that the motor gear ratios are 
7T, r 2 , respectively. 

9-3 What are the natural and artificial constraints for the task of inserting a 
square peg into a square hole? Sketch the compliance frame for this task. 

9-4 Describe the natural and artificial constraints associated with the task of 
opening a box with a hinged lid. Sketch the compliance frame. 

9-5 Discuss the task of opening a long two-handled drawer. How would you go 
about performing this task with two manipulators? Discuss the problem 
of coordinating the motion of the two arms. Define compliance frames for 
the two arms and describe the natural and artificial constraints. 


9-6 Given the following tasks, classify the environments as either Inertial , 


Capacitive , or Resistive according to Definition 9.2 


1. Turning a crank 

2. Inserting a peg in a hole 

3. Polishing the hood of a car 

4. Cutting cloth 
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5. Shearing a sheep 

6. Placing stamps on envelopes 

7. Cutting meat 
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GEOMETRIC 

NONLINEAR 

CONTROL 


10.1 INTRODUCTION 

In this chapter we present some basic, but fundamental, ideas from geomet- 
ric nonlinear control theory. We first discuss the notion of feedback lin- 
earization of nonlinear systems. This approach generalizes the concept of 
inverse dynamics of rigid manipulators discussed in Chapter [8] The basic idea 
of feedback linearization is to construct a nonlinear control law as a so-called 
inner loop control which, in the ideal case, exactly linearizes the nonlinear 
system after a suitable state space change of coordinates. The designer can 
then design a second stage or outer loop control in the new coordinates to 
satisfy the traditional control design specifications such as tracking, disturbance 
rejection, and so forth. 

In the case of rigid manipulators the inverse dynamics control of Chapter [8] 
and the feedback linearizing control are the same. However, as we shall see, 
the full power of the feedback linearization technique for manipulator control 
becomes apparent if one includes in the dynamic description of the manipulator 
the transmission dynamics, such as elasticity resulting from shaft windup and 
gear elasticity. 

We also give an introduction to modeling and controllability of nonholonomic 
systems. We treat systems such as mobile robots and other systems subject to 
constraints arising from conservation of angular momentum or rolling contact. 
We discuss the controllability of a particular class of such systems, known as 
driftless systems. We present a result known as Chow’s Theorem, which 
gives a sufficient condition for local controllability of driftless systems. 
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10.2 BACKGROUND 

In this section we give some background from differential geometry that is 
necessary to understand the results to follow. In recent years an impressive 
volume of literature has emerged in the area of differential geometric methods 
for nonlinear systems, treating not only feedback linearization but also other 
problems such as disturbance decoupling, estimation, etc. The reader is referred 
to [?] for a comprehensive treatment of the subject. It is our intent here to give 
only that portion of the theory that finds an immediate application to robot 
control, and even then to give only the simplest versions of the results. 

The fundamental notion in differential geometry is that of a differentiable 
manifold (manifold for short) which is a topological space that is locally dif- 
feomorphiqj to Euclidean space, R m . For our purposes here a manifold may be 
thought of as a subset of R” defined by the zero set of a smooth vector valued 
functioifi] h =: R" — > R p , for p < n, i.e. 

hi(xi,...,x n ) = 0 
hp (Xl , . . . , X n ) — 0 

We assume that the differentials dhi, . . . ,dh p are linearly independent at each 
point in which case the dimension of the manifold is to = n — p. Given an 
TO-dimensional manifold, M, we may attach at each point x £ M a tangent 
space, T X M , which is an m-dimensional vector space specifying the set of 
possible velocities (directinal derivatives) at x. 

Definition 10.1 A smooth vector field on a manifold M is a function 
f : M — » T X M which is infinitely differentiable, represented as a column 
vector, 


fix) 


h{x) 

fm{x) _ 


Another useful notion is that of cotangent space and covector field. The 
cotangent space, T*M, is the dual space of the tangent space. It is an TO- 
dimensional vector space specifying the set of possible differentials of functions 
at x. Mathematically, T*M is the space of all linear functionals on T X M , i.e., 
the space of functions from T X M to R. 


1 A diffeomorphism is simply a differentiable function whose inverse exists and is also dif- 
ferentiable. We shall assume both the function and its inverse to be infinitely differentiable. 
Such functions are customarily referred to as C°° difFeomorphisms 

2 Our definition amounts to the special case of an embedded submanifold of dimension 
m — n — p in R n 
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Definition 10.2 A smooth covector field is a function w : M — > T*M 

which is infinitely differentiable, represented as a row vector, 

w(x) = [ wi (x)...w m (x) ] 

Henceforth, whenever we use the term function, vector field, or covector field, it 
is assumed to be smooth. Since T X M and T*M are m-dimensional vector spaces, 
they are isomorphic and the only distinction we will make between vectors and 
covectors below is whether or not they are represented as row vectors or column 
vectors. 

Example 10.1 Consider the unit sphere, S 2 , in R 3 defined by 
h(x, y, z) = x 2 + y 2 + z 2 — 1 = 0 

S 2 is a two-dimensional submanifold ofM 3 . At points in the upper hemisphere, 



Fig. 10.1 The sphere as a manifold in R 3 


z = Vi — x 2 — y 2 , the tangent space is spanned by the vectors 


Vi = (1,0, -x/y/l - x 2 - y 2 ) T 

(10.1) 

v 2 = (0, 1 , —y/y/l — x 2 — y 2 ) T 

(10.2) 

The differential of h is 


dh = (2x, 2 y, 2 z) = (2a;, 2 y, 2\/l — x 2 — y 2 ) 

(10.3) 


which is easily shown to be normal to the tangent plane at x, y, z. o 

We may also have multiple vector fields defined simultaneously on a given 
manifold. Such a set of vector fields will fill out a subspace of the tangent space 
at each point. Likewise, we will consider multiple covector fields spanning a 
subspace of the cotangent space at each point. These notions give rise to so- 
called distributions and codistributions. 
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Definition 10.3 

1. Let Xi(x), . . . , Xk(x) be vector fields on M , which are linearly independent 
at each point. By a Distribution A we mean the linear span (at each 
x e M) 

A = span {Xi(x), . . . ,X k (x)} (10.4) 

2. Likewise, let w i(x), . . . ,Wk(x) be covector fields on M, which are linearly 
independent at each point. By a Codistribution f l we mean the linear 
span ( at each x £ M) 

ft = span{w 1 (a;), . . . ,u’k(x)} (10.5) 


A distribution therefore assigns a vector space A (a;) at each point x £ M; a 
fc-dimensional subspace of the m-dimensional tangent space T X M . A codistri- 
bution likewise defines a /c-dimensional subspace at each x of the ?n-dimensional 
cotangent space T*M. The reader may consult any text on differential geome- 
try, for example [?], for more details. 

Vector fields are used to define differential equations and their associated 
flows. We restrict our attention here to nonlinear systems of the form 

x = f(x) + g 1 {x) 1 u + ...g m (x)um 

=: /( x) + G(x)u ( 10 . 6 ) 

where G(x) = [gi{x), . . ,,g m (x)], u = (u u . . .,u m ) T , and f(x),g i(x), . . .,g m {x) 
are vector fields on a manifold M. For simplicity we will assume that M = R n . 


Definition 10.4 Let f and g be two vector fields on R n . The Lie Bracket of 
f and g, denoted by [/, g], is a vector field defined by 


[fid] 


dg f _df 

dx J dx 9 


( 10 . 7 ) 


where ( respectively , denotes the n x n Jacobian matrix whose ij-th 


entry is (respectively, ^ f -). 


Example 10.2 Suppose that vector fields f(x) and g( x) on R 3 are given as 



X2 


0 

f(x) = 

sinxi 

g(x) = 

x\ 


_ x\ + Xi 


0 

Then the vector field [/, g\ is computed 

according to 

(10. 7) 


[f,g\ 


' 0 0 O' 


x 2 


0 1 0 


0 

0 2x 2 0 


sinaq 

- 

cos x\ 0 0 


x\ 

0 0 0 


_ Xi + x\ 


1 0 2x 3 


1 


2 x 2 sinaq 
-2x 3 
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O 

We also denote [/, g] as adf(g) and define adj(g) inductively by 

ad k f (g) = [/, ad^~ 1 (g)] (10.8) 

with ad°f(g) = g. 

Definition 10.5 Let f : R n — » R" be a vector field on R" and let h : R ra — > R 
be a scalar function. The Lie Derivative of h, with respect to f, denoted Lfh, 
is defined as 

Lfh = £ /(x) = (1 °' 9) 

i— 1 

The Lie derivative is simply the directional derivative of h in the direction of 
f(x), equivalently the inner product of the gradient of h and /. We denote by 
L 2 h the Lie Derivative of Lfh with respect to /, i.e. 

L 2 fh = Lf(Lfh) (10.10) 

In general we define 

L)h = L f {L)~ x h) for fc = 1, . . . , n (10.11) 

with L°j:h = h. 

The following technical lemma gives an important relationship between the 
Lie Bracket and Lie derivative and is crucial to the subsequent development. 

Lemma 10.1 Let h : R n — > R be a scalar function and f and g be vector fields 
on R". Then we have the following identity 

I J {f,g}h = LfLgh — L g Lfh ( 10 . 12 ) 


Proof: Expand Equation (10.12) in terms of the coordinates xi,...,x n and 
equate both sides. The z-th component [/. g], of the vector field [f,g] is given 
as 

[, 1 _ f ST' ofi 

Ifi9h ~ a . 9j 


1=1 


1=1 


Therefore, the left-hand side of (|10. 12 1 is 


dh 


L U,g] h = Y,q x \^9] 


dh 


E un I \ ^ f — V'' 

Cl r I 2-^ Onr _■ l 2-^d 


dx 


i=i 
n n 


i!=l 


dx-j 


l=i 


dfi 

dx 


9j 


sr' ST f _ d/i 

2^2^;), I ;i ■' > ~ !l ' 


1=1 1=1 


304 


GEOMETRIC NONLINEAR CONTROL 


If the right hand side of ( |10.12| ) is expanded similarly it can be shown, with a 
little algebraic manipulation, that the two sides are equal. The details are left 
as an exercise (Problem 10-[T|) . 


10.2.1 The Frobenius Theorem 


In this section we present a basic result in differential geometry known as the 
Frobenius Theorem. The Frobenius Theorem can be thought of as an exis- 
tence theorem for solutions to certain systems of first order partial differential 
equations. Although a rigorous proof of this theorem is beyond the scope of this 
text, we can gain an intuitive understanding of it by considering the following 
system of partial differential equations 

f(x,y,z) (10.13) 

g(x,y,z) (10.14) 


dz 

dx 

dz 

dy 


In this example there are two partial differential equations in a single dependent 
variable z. A solution to ( 10.13 1-( 10.14) is a function z = </>(x,y) satisfying 


d(j> 

dx 

dcj> 

dy 


f(x,y,<l>(x,y)) 

g{x,y,4>{x,y)) 


(10.15) 

(10.16) 


We can think of the function 2 = <j){x, y) as defining a surface in 
Figure 10.2 The function 4> : R 2 — > R 3 defined by 


as m 



Fig. 10.2 Integral manifold in R 3 


®{x,y) 


(x,y,(t>{x,y)) 


(10.17) 


BACKGROUND 305 


then characterizes both the surface and the solution to the system of Equa- 


tions ( 10.13 1-( 10.14 1. At each point (, x,y ) the tangent plane to the surface is 
spanned by two vectors found by taking partial derivatives of $ in the x and y 
directions, respectively, that is, by 


Xi = (1,0 ,f(x,y,<p(x,y))) T 

X 2 = (0,1 ,g(x,y,<j)(x,y))) T 


(10.18) 


The vector fields A'i and X 2 are linearly independent and span a two dimen- 
sional subspace at each point. Notice that X\ and X 2 are completely specified 



If such an integral manifold exists then the set of vector fields, equivalently, the 
system of partial differential equations, is called completely integrable. 

Let us reformulate this problem in yet another way. Suppose that z = 4>{x, y) 
is a solution of (10.13)-(10.14l. Then it is a simple computation (Problem 10§ 
to check that the function 


h(x,y,z) = z — 4>(x, y) 

satisfies the system of partial differential equations 

Lx i h = 0 
L x 2 h = 0 


(10.19) 


( 10 . 20 ) 


Conversely, suppose a scalar function h can be found satisfying (10.201, and 
suppose that we can solve the equation 


h{x,y,z) = 0 


( 10 . 21 ) 


for z, as z = </>(x, ?/)(^] Then it can be shown that cf> satisfies ( 10.13 1-(10.14 ). 
(Problem 10§ Hence, complete integrability of the set of vector fields (Xl, X 2 ) 
is equivalent to the existence of h satisfying (10.201. With the preceding dis- 
cussion as background we state the following 


Definition 10.6 A distribution S = span{ X ±, ..., X m } on R n is said to be 
completely integrable if and only if there are n — m linearly independent 
functions hi, , h n - m satisfying the system of partial differential equations 

L x , hj = 0 for 1 < i < n ; 1 < j < to (10.22) 


3 The 
„„ dh 

33 m 


so-called bf Implicit Function Theorem states that jl0.2l| l can be solved for z as long 

AO. 
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Definition 10.7 A distribution S = span{X i, . . . ,X m } is said to be involu- 
tive if and only if there are scalar functions : R" — > R such that 

m 

[Xi,Xj] = Y] a ijk X k for all i,j,k (10.23) 

k = 1 


Involutivity simply means that if one forms the Lie Bracket of any pair of 
vector fields in A then the resulting vector field can be expressed as a linear 
combination of the original vector fields X\, , X m . Note that the coefficients 
in this linear combination are allowed to be smooth functions on R™. In the 
simple case of ( 10.13 1- ( 10.14 1 one can show that if there is a solution z = </>( x, y) 
of ( 10.13| )-(10.14l then involutivity of the set {Xi,^} defined by (10.221 is 
equivalent to interchangeability of the order of partial derivatives of </>, that is, 

■ a a = tt t ■ The Frobenius Theorem, stated next, gives the conditions for 
the existence of a solution to the system of partial differential Equations ( 10.22 1. 


Theorem 2 A distribution A is completely integrable if and only if it is invo- 
lutive. 


Proof: See, for example, Boothby [?]. 


10.3 FEEDBACK LINEARIZATION 

To introduce the idea of feedback linearization consider the following simple 
system, 


i’i = asin(a:2) 

X2 = — X\ + U 


(10.24) 

(10.25) 


Note that we cannot simply choose u in the above system to cancel the nonlinear 
term a sin (a^)- However, if we first change variables by setting 


V l = x i 

j/2 = asin(a;2) = X\ 

then, by the chain rule, j/i and 2/2 satisfy 
V 1 = 2/2 

ij 2 = acos(x 2 )(— x\ + u) 

We see that the nonlinearities can now be cancelled by the input 

1 .2 

v + aq 


tcos(*2) 


(10.26) 

(10.27) 


(10.28) 


(10.29) 
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which result in the linear system in the (j/ 1 , 2 / 2 ) coordinates 


2/ 1 = 

2/2 = 


2/2 

v 


(10.30) 


The term v has the interpretation of an outer loop control and can be de- 


signed to place the poles of the second order linear system ( 10.30 1 in the coor- 
dinates ( 2 / 1 : 2 / 2 ) • For example the outer loop control 


v = -kiyi - k 2 y 2 


applied to (10.301 results in the closed loop system 


2/i = 2/2 

2/2 = -kiyi - k 2 y 2 

which has characteristic polynomial 

p(s) = s 2 + k 2 s + k\ 


(10.31) 


(10.32) 


(10.33) 


and hence the closed loop poles of the system with respect to the coordi- 


nates ( 2 / 1 , 2 / 2 ) are completely specified by the choice of k\ and k 2 . Figure 10.3 
illustrates the inner loop/outer loop implementation of the above control strat- 



Nonlinear 

coordinate 

transformation 


Fig. 10.3 Feedback linearization control architecture 


egy. The response in the y variables is easy to determine. The corresponding 
response of the system in the original coordinates (x\,x 2 ) can be found by 
inverting the transformation (10.26 (-(10.27), in this case 

Xl = V } _ 1( , , (10.34) 

x 2 = sin (2/2/a) — a < y 2 < +a 

This example illustrates several important features of feedback linearization. 
The first thing to note is the local nature of the result. We see from (10.261 
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and (10.27) that the transformation and the control make sense only in the 
region — oo < X\ < oo, — \ < X 2 < f ■ Second, in order to control the linear 
system ( 10.30 1, the coordinates (y 1 , 2 / 2 ) must be available for feedback. This can 
be accomplished by measuring them directly if they are physically meaningful 
variables, or by computing them from the measured ( Xi,X 2 ) coordinates using 
the transformation ( 10.26 >-( 10.27 1 . In the latter case the parameter a must be 
known precisely. 

In Section [lOT] give necessary and sufficient conditions under which a general 
single-input nonlinear system can be transformed into a linear system in the 
above fashion, using a nonlinear change of variables and nonlinear feedback as 
in the above example. 


10.4 SINGLE-INPUT SYSTEMS 


The idea of feedback linearization is easiest to understand in the context of 
single-input systems. In this section we derive the feedback linearization result 
of Su [?] for single-input nonlinear systems. As an illustration we apply this 
result to the control of a single-link manipulator with joint elasticity. 


Definition 10.8 A single-input nonlinear system 


x = f(x)+g(x)u (10.35) 

where f(x) and g( x) are vector fields on R ra , /( 0) = 0, and u £ R, is said to be 
feedback linearizable if there exists a diffeomorphism T : U — > R”, defined 
on an open region U in R n containing the origin , and nonlinear feedback 


u = a(x) + /3(x)v (10.36) 

with (3(x) ^ 0 on U such that the transformed variables 


V = T{ x) 

satisfy the linear system of equations 

y = Ay + bv 

where 



' 0 

1 

0 

0 ' 


' 0 ' 


0 

0 

1 



0 

A = 





b = 






1 




0 

0 


0 0 


1 


(10.37) 

(10.38) 


(10.39) 


Remark 10.1 The nonlinear transformation (10.31) and the nonlinear control 
law (10.36), when applied to the nonlinear system (10.35), result in a linear 
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controllable system (10.38). The diffeomorphism T{x) can be thought of as a 
nonlinear change of coordinates in the state space. The idea of feedback lin- 
earization is then that if one first changes to the coordinate system y = T(x), 
then there exists a nonlinear control law to cancel the nonlinearities in the sys- 
tem. The feedback linearization is said to be global if the region U is all of 


We next derive necessary and sufficient conditions on the vector fields / and 

(10.40) 


g in (10.35) for the existence of such a transformation. Let us set 

V = T{x) 


and see what conditions the transformation T(x) must satisfy. Differentiating 
both sides of (10.401 with respect to time yields 


dT 

y = -w-x 
ox 


(10.41) 


fi'T' 

where HA- is the Jacobian matrix of the transformation T(x). Using (10.351 
and (10.381, Equation (10.411 can be written as 


dT 

dx 


In component form with 

' T-i 

T = 

T n 


(f(x) + g{x)u ) = Ay + bv 


0 10 
0 0 1 


(10.42) 


A = 


0 0 


we see that the first equation in ( 10.42 1 is 


dTi . 
dx 


Xi 


dr 1 

dx' 


1 

0 0 


= T 2 


II 

-o 

1 

1 

0 o • 

1 


1 


(10.43) 


(10.44) 


which can be written compactly as 


L f Ti + L g Tiu = T 2 (10.45) 

Similarly, the other components of T satisfy 

LfT 2 + L g T 2 u = T 3 

LfT n + L g T n u = 


v 


(10.46) 
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Since we assume that Tf , . . . , T n are independent of u while v is not independent 
of u we conclude from (10.461 that 


LgT\ LgT2 * * * LgT n -l — 0 

LgT n ^ o 

This leads to the system of partial differential equations 
L f Ti = T i+ 1 i = 1, . . . n — 1 

together with 

L f T n + L g T n u = v 


(10.47) 

(10.48) 


(10.49) 


(10.50) 


Using Lemma 10.1 and the conditions (10.471 and (10.481 we can derive a 
system of partial differential equations in terms of Tf alone as follows. Using 
h = T\ in Lemma |10.1| we have 


L [. f,g] T i = L f L g T i - L g L f T i = 0 - L g T 2 = 0 
Thus we have shown 


(10.51) 




Ti = 0 


By proceeding inductively it can be shown (Problem 10-[4j) that 


L 


T\ = 0 fc = 0,l,...n — 2 


(10.52) 

(10.53) 

(10.54) 

If we can find T) satisfying the system of partial differential Equations (10.531, 
then T 2 , . . . ,T n are found inductively from (10.491 and the control input u is 
found from 


adHg 


La^gT, ± 0 


LfT n + L g T n u = v 


(10.55) 


as 


u = 


L g T n 


( V - LfTn) 


(10.56) 


We have thus reduced the problem to solving the system (10.531 for T\. 
When does such a solution exist? 

First note that the vector fields g, adf(g), . . . ,ad^~ (g) must be linearly 
independent. If not, that is, if for some index i 


i—l 

ad){g) = '^2,a k ad){g ) 

k = 0 


(10.57) 
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then ad ” 1 


(g) would be a linear combination of g, adf(g), . . . ,adj~ ( g ) and 
(10.541 could not hold. Now by the Frobenius Theorem ( 10.53| ) has a solution 
if and only if the distribution A = spanjg, a df(g), . . . , ad r ’j^ l (g ) } is involutive. 
Putting this together we have shown the following 


Theorem 3 The nonlinear system 

x = f{x) + g(x)u (10.58) 

with f(x), g(x) vector fields, and /( 0) = 0 is feedback linearizable if and only 
if there exists a region U containing the origin in R ra in which the following 
conditions hold: 

1. The vector fields { g , adf(g), . . . , adj _1 (g)} are linearly independent in U 

2. The distribution span {g,adf(g), . . . ,ad™~ (g)} is involutive in U 


Example 10.3 [1] Consider the single link manipulator with flexible joint 
shown in Figure{l0.4\ Ignoring damping for simplicity, the equations of motion 


Ql I, M 



Fig. 10.4 Single-Link Flexible Joint Robot 


are 


Iq i + Mgl sin fa) + k{q x - q 2 ) = 0 _ _ 

Jq 2 + k(q 2 - qi) = u ' ' 

Note that since the nonlinearity enters into the first equation the control u 
cannot simply be chosen to cancel it as in the case of the rigid manipulator 
equations. 

In state space we set 


Xi = q i 

X3 = q 2 


x 2 = q i 
X4 = qi 


(10.60) 
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and write the system (10.59) as 


Xi 

= X 2 

X 2 

1 

i'3 

= X4 

£4 

= l( 


sin(a;i) - ){x 3 - x 3 ) 


(10.61) 


The system is thus of the form (10.35) with 



X2 



0 

fix) = 

M f sinOn) - 

£4 

ji x l - x 3 ) 

9(x) = 

0 

0 


i 

^l?r 

i 

X 3 ) 


1 

. J _ 


(10.62) 


Therefore n = 4 and the necessary and sufficient conditions for feedback lin- 
earization of this system are that 


and that the set 


rank [g, ad f (g), ad 2 {g), ad 3 f (g)] = 4 


{g,adf{g),ad 2 Ag)} 


(10.63) 


(10.64) 


(Problem 10® 

[g,adf(g),ad 2 f (g),ad 3 f (g)\ = 


0 0 
0 0 
0 7 
7 0 


it is 

easy to 

0 

k 

IJ 

k 

0 

IJ 

0 

k 

J 2 

k 

J 2 

0 


(10.65) 


which has rank 4 for k > 0, I, J < oo. Also, since the vector fields {g,adf(g), 
ad 2 (g)} are constant, they form an involutive set. To see this it suffices to note 
that the Lie Bracket of two constant vector fields is zero. Hence the Lie Bracket 
of any two members of the set of vector fields in (10.64) zer ° which is trivially 
a linear combination of the vector fields themselves. It follows that the system 
(10.59) is feedback linearizable. The new coordinates 

y i = T i * = ,4 (10.66) 


are found from the conditions ( 10.53), with n = 4, that is 


L g Ti = 0 

L lf,g ] T i = 0 

L ad 2 f g T l = 0 

L adfg T l = 0 


(10.67) 
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Carrying out the above calculations leads to the system of equations (Prob- 
lem 10% 


"1=0. "1=0- "i= 0 

o 1 r-, W 1 r\ W 

<7#2 <7X3 <7^4 


and 


dTi 

dx\ 


^ 0 


( 10 . 68 ) 


(10.69) 


From this we see that the function T) should be a function of x 4 alone. There- 
fore, we take the simplest solution 


2/i = T x = x x 


and compute from (10.49) (Problem 10-10) 


2/2 = T ‘2 = L f T\ = x 2 

2/3 = T 3 = L f T 2 = -^sm(x 1 )-f(x 1 ^x 3 ) 
2/4 = X 4 = L f T 3 = cos(zi) - j(x 2 - x 4 ) 


TTie feedback linearizing control input u is found from the condition 

1 (v-LfTt) 


LgTx 


as (Problem 10-11 ) 


u = -—(v — a(x)) = f3(x)v + a(x) 

k 


where 


a(x) := MlLi sin (a; 4 ) ( x\ + C os(a;i) + ^ 


MgL 

T 


+7(^1 — X3) — + — + 


1 


k k MgL 


I J 


COs(lTi) 


(10.70) 


(10.71) 


(10.72) 


(10.73) 


(10.74) 


Therefore in the coordinates ?/i, • • • , 2/4 with the control law (10.731 the sys- 
tem becomes 


2/i = 2/2 

2/2 = 2/3 

2/3 = 2/4 

y 4 = v 


(10.75) 


or, in matrix form, 


2 / = 


Ay + bv 


(10.76) 
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where 


' 0 

1 

0 

0 ' 


' 0 ' 

0 

0 

1 

0 

E 

0 

0 

0 

0 

1 

0 — 

0 

1 

0 

0 

0 

1 

0 


1 


(10.77) 


Remark 10.2 The above feedback linearization is actually global. In order to 
see this we need only compute the inverse of the change of variables (10.70)- 
(10.71). Inspecting (10.70)-(10.71) we see that 


X\ 

= 9 1 




X 2 

= 92 





, I 

( , 

MqL 


X3 

= m + k ! 

[93 + 

I 

sm(yi) J 


= » + ( 

f 2/4 + 

MgL 

I 

cos (2/1 )y ; 


(10.78) 


The inverse transformation is well defined and differentiable everywhere and , 
hence, the feedback linearization for the system (10.59) holds globally. The 
transformed variables y \, . . . , y 4 are themselves physically meaningful. We see 
that 


91 = Xi 

92 = x 2 

93 = 92 

94 = 93 


link position 
link velocity 
link acceleration 
link jerk 


(10.79) 


Since the motion trajectory of the link is typically specified in terms of these 
quantities they are natural variables to use for feedback. 


Example 10.4 One way to execute a step change in the link position while 
keeping the manipulator motion smooth would be to require a constant jerk 
during the motion. This can be accomplished by a cubic polynomial trajectory 
using the methods of Chapter \5.0\ Therefore, let us specify a trajectory 

0f{t) = pi = ai + a 2 t + a 3 t 2 + aff 3 (10.80) 

so that 

92 = 9i = a 2 + 2 ci 3 1 + 3a 4 t 2 

93 = 92 = 2ft 3 + 6 a 4 t 

94 = 93 = 6 «4 

Then a linear control law that tracks this trajectory and that is essentially equiv- 
alent to the feedforward/feedback scheme of Chapter^ is given by 

= yf - ki(yi - yi) - k 2 (y 2 - yi) - k 3 (y 3 - 93) - MP 4 ~ vi) ( 10 . 81 ) 


V 
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Applying this control law to the fourth order linear system (10.73) we see that 
the tracking error eft) = yi — yf satisfies the fourth order linear equation 




~re , d 3 e , d 2 e , tie 
— + k 4 — + k 3 — + k2 - + k 1 e - 0 


(10.82) 


and, hence, the error dynamics are completely determined by the choice of gains 


Notice that the feedback control law (10.811 is stated in terms of the vari- 
ables j/i , . . . , y\. Thus, it is important to consider how these variables are to be 
determined so that they may be used for feedback in case they cannot be mea- 
sured directly. Although the first two variables, representing the link position 
and velocity, are easy to measure, the remaining variables, representing link ac- 
celeration and jerk, are difficult to measure with any degree of accuracy using 
present technology. One could measure the original variables xi , ... ,£4 which 
represent the motor and link positions and velocities, and compute 2 / 1 , - - - > 3/4 


using the transformation Equations ( 10.70 1- ( 10.71 1. In this case the parameters 


appearing in the transformation equations would have to be known precisely. 
Another, and perhaps more promising, approach is to construct a dynamic 
observer to estimate the state variables 2 / 1 , • ■ • , 2 / 4 - 


10.5 FEEDBACK LINEARIZATION FOR iV-LINK ROBOTS 


In the general case of an n-link manipulator the dynamic equations represent 
a multi-input nonlinear system. The conditions for feedback linearization of 
multi-input systems are more difficult to state, but the conceptual idea is the 
same as the single-input case. That is, one seeks a coordinate systems in which 
the nonlinearities can be exactly canceled by one or more of the inputs. In the 
multi-input system we can also decouple the system, that is, linearize the system 
in such a way that the resulting linear system is composed of subsystems, each 
of which is affected by only a single one of the outer loop control inputs. Since 
we are concerned only with the application of these ideas to manipulator control 
we will not need the most general results in multi-input feedback linearization. 
Instead, we will use the physical insight gained by our detailed derivation of 
this result in the single-link case to derive a feedback linearizing control both 
for n-link rigid manipulators and for n-link manipulators with elastic joints 
directly. 


Example 10.5 We will first verify what we have stated previously, namely 
that for an n-link rigid manipulator the feedback linearizing control is identical 
to the inverse dynamics control of Chapter [#[ To see this, consider the rigid 
equations of motion (8.6), which we write in state space as 


= x 2 

= -~M{x\)~ 1 (C(x\,X 2 )x 2 + g{x\)) + M(x\)~ 1 U 


X\ 

x 2 


(10.83) 
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with X\ = q; X2 = q. In this case a feedback linearizing control is found by 

as 

u = M(xi)v + C(xi, x 2 )x 2 + g(xi) (10.84) 


simply inspecting (10.83) as 


Substituting (10.84) i- n t° (10.83) yields 


xi = x 2 

x 2 = v 


(10.85) 


Equation (10.85) represents 


a set of n-second order systems of the form 


•Eli — %2i 

±2 i = Vi, i = l,...,n 


( 10 . 86 ) 


Comparing (10. 84) with (8.11) we see indeed that the feedback linearizing control 
for a rigid manipulator is precisely the inverse dynamics control of Chapter [S| 


Example 10.6 If the joint flexibility is included in the dynamic description of 
an n-link robot the equations of motion can be written as[l] 


D(Qi)Qi + C(qiiQi)ii + g{qi) + E(qi — q 2 ) — 0 

Jq2~K{q 1 ~q 2 ) = u 


(10.87) 


In state space, which is now M 4 ", we define state variables in block form 


Xi = qi x 2 = q 1 

X3 = 02 x 4 = 02 


( 10 . 88 ) 


Then from (10.81) 


we have: 


x\ = x 2 

x 2 = -D(x 1 )- 1 {h(x 1 ,x 2 ) + K{xi - x 3 )} 

x 3 = x 4 

x 4 = J- X K [x\ — X 3 ) + J~ 1 u 


(10.89) 


ere we define h(x i,x 2 ) = C(x i,x 2 )x 2 + g(x 1 ) for simplicity. This system is 
then of the form 


x = /( x) + G(x)u (10.90) 

In the single-link case we saw that the appropriate state variables with which 
to define the system so that it could be linearized by nonlinear feedback were 
the link position, velocity, acceleration, and jerk. Following the single-input 
example, then, we can attempt to do the same thing in the multi-link case and 
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derive a feedback linearizing transformation blockwise as follows: Set 

Vi = Ti(xi) := xi 

y 2 = T 2 (x):=y 1 =x 2 

2/3 = T 3 (x) := y 2 = x 2 

= —D(x{)~ 1 {h(xi,x 2 ) + K(x\ — :r 3 )} 

2/4 = T 4 (x) := y 3 (10.91) 

= --gf[D(x 1 )- 1 ]{h(x 1 ,x 2 ) + Kfa - x 3 )} - D(x 1 )~ 1 x 2 

+ §f 2 [- D (x l y 1 {h{x 1 ,x 2 ) +K(x 1 — £ 3 ))] +K(x 2 -ar 4 )| 

:= a 4 (xi, x 2 , x 3 ) + H(a;i) _ 1 A" 3;4 

where for simplicity we define the function a 4 to be everything in the definition 
of y 4 except the last term, which is D~ 1 I\x 4 . Note that x 4 appears only in this 
last term so that a 4 depends only on x\,x 2 ,x 3 . 

As in the single-link case, the above mapping is a global diffeomorphism. Its 
inverse can be found by inspection to be 


xi = 2/i 

X2 = 2/2 

x 3 = 2/1 + (2/1) 2/3 + M2/I72/2)) 

X4 = K~ 1 D(yi)(y4 — a 4 (j/i, y 2 , y 3 )) 

The linearizing control law can now be found from the condition 

y 4 = v 


(10.92) 


(10.93) 


where v is a new control input. Computing y 4 from (10.91) and suppressing 
function arguments for brevity yields 


v = mr 1 x *-^ D - 1 (h + K(x 1 -x 3 )) (10.94) 

+ + yf[D~ 1 ]Kx 4 + D~ 1 K(J- 1 K(x 1 - x 3 ) + J _ 1 m) 

=: a(x ) + b(x)u 


where a(x) denotes all the terms in \10.94 ) 
the input u, and b(x) := D~ 1 (x)KJ~. 
Solving the above expression for u yields 


but the last term, which involves 


u = b(x) 1 (v — a(x)) =: a(x) + f3(x)v (10.95) 


where f3{x) = JK~ 1 D{x) and a{x) = — 6 (a;) _ 1 a(x). 

With the nonlinear change of coordinates \10.91 ) and nonlinear feedback 
(10.95) the transformed system now has the linear block form 


' 0 

I 

0 

0 ' 


' 0 ' 

0 

0 

I 

0 

y + 

0 

0 

0 

0 

I 

0 

0 

0 

0 

0 


1 


=: Ay + Bv 


(10.96) 
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where I = nxn identity matri x , 0 = n x n zero matrix, y T = (yf , y%, y$, yj) £ 
R 4ra , and v £ K". The system (10.96) represents a set of n decoupled quadruple 
integrators. The outer loop design can now proceed as before, because not only 
is the system linearized, but it consists of n subsystems each identical to the 
fourth order system (10.75). o 


10.6 NONHOLONOMIC SYSTEMS 


In this section we return to a discussion of systems subject to constraints. A 
constraint on a mechanical system restricts its motion by limiting the set of 
paths that the system can follow. We briefly discussed so-called holonomic 
constraints in Chapter [6] when we derived the Euler-Lagrange equations of 
motion. Our treatment of force control in Chapter [9] dealt with unilateral 
constraints defined by the environmental contact. In this section we expand 
upon the notion of systems subject to constraints and discuss nonholonomic 
systems. 

Let Q denote the configuration space of a given system and let q = [q\, . . . q n ) T 
£ Q denote the vector of generalized coordinates defining the system configu- 
ration. We recall the following definition. 

Definition 10.9 A set of k < n constraints 

hi(q\, . . . ,q n ) = 0 i=l,...,k (10.97) 


is called holonomic, where hi is a smooth mapping from Q i— > R. 

We assume that the constraints are independent so that the differentials 


dhi 


dhi dh\ 
dqi''"' dq n 


dh k 


dh k dh k 
dqi ’ ' " ’ dq n 


are linearly independent covectors. Note that, in order to satisfy these con- 
straints, the motion of the system must lie on the hypersurface defined by the 
functions hi,... h k , i.e. 


hi(q(t)) = 0 for all £ > 0 


As a consequence, by differentiating (10.981, we have 


< dhi, q >= 0 i = 1, . . . , k 


(10.98) 


(10.99) 


where < •, • > denotes the standard inner product. 
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It frequently happens that constraints are expressed, not as constraints on 


the configuration, as in (10.971, but as constraints on the velocity 
< Wi, q >= 0 i = 1 , . . . , k 


( 10 . 100 ) 


where Wi(q) are covectors. Constraints of the form (10.100) are known as 
Pfaffian constraints. The crucial question in such cases is, therefore, when 
can the covectors wi,...Wk be expressed as differentials of smooth functions, 
hi, . . . , hk7 We express this as 


Definition 10.10 Constraints of the form 

< Wi, q >= 0 i = 1 , . . . k 

are called holonomic if there exists smooth functions hi, 
Wi(q) = dhi(q) i = l,...k 


( 10 . 101 ) 
, /ifc such that 

( 10 . 102 ) 


and nonholonomic otherwise, i.e. if no such functions hi, ... h^ exist. 

We can begin to see a connection with our earlier discussion of integrability and 
the Frobenius Theorem if we think of Equation ( 10.102 1 as a set of partial differ- 
ential equations in the (unknown) functions hi. Indeed, the term integrable 
constraint is frequently used interchangeably with holonomic constraint for 
this reason. 


10.6.1 Involutivity and Holonomy 

Now, given a set of Pfaffian constraints < Wi(q),q > i = 1 ,...k, let f 1 be 
the codistribution defined by the covectors wi, . . . ,Wk and let {171 , . . . , g m } for 
m = n — k be a basis for the distribution A that annihilates Ll, i.e. such that 


< Wi,gj >= 0 for each i, j 


(10.103) 


We use the notation A = fU (This is pronounced “fl perp”). 
Equation (10.1021 that 


Notice from 


0 =< Wi,gj >=< dhi,gj > for each i,j 


(10.104) 


Using our previous notation for Lie Derivative, the above system of equations 
may be written as 


L g . hi = 0 i = 1, . . . , k; j = 1, . . . ,m (10.105) 

The following theorem thus follows immediately from the Frobenius Theorem 

Theorem 4 Let LI be the codistribution defined by covectors wi, . . . , w^. Then 
the constraints < Wi,q >= 0 i = 1 ,...,k are holonomic if and only if the 
distribution A = f] 1 - is involutive. 
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10.6.2 Driftless Control Systems 

It is important to note that the velocity vector q of the system is orthogonal to 


the covectors Wi according to (10.101) and hence lies in the distribution A = 
In other words the system velocity satisfies 


q = gi(q)u! 


9m{q)i 


(10.106) 


for suitable coefficients u\, . . . ,u m . In many systems of interest, the coeffi- 


cients Ui in (10.1061 have the interpretation of control inputs and hence Equa- 


tion ( 10.106 1 defines a useful model for control design in such cases. The system 


(10.1061 is called driftless because q = 0 when the control inputs U \, . .. ,u m 
are zero. In the next section we give some examples of driftless systems arising 
from nonholonomic constraints, followed by a discussion of controllability of 


driftless systems and Chow’s Theorem in Section 10.7 


10.6.3 Examples of Nonholonomic Systems 

Nonholonomic constraints arise in two primary ways: 

1. In so-called rolling without slipping constraints. For example, the 
translational and rotational velocities of a rolling wheel are not indepen- 
dent if the wheel rolls without slipping. Examples include 

• A unicycle 

• An automobile, tractor/trailer, or wheeled mobile robot 

• Manipulation of rigid objects 

2. In systems where angular momentum is conserved. Examples include 

• Space robots 

• Satellites 

• Gymnastic robots 

Example: 10.7 The Unicycle. The unicycle is equivalent to a wheel rolling 
on a plane and is thus the simplest example of a nonholonomic system. Refering 
to Figure \10.S\ we see that the configuration of the unicycle is defined by the 
variables x,y, 6 and <f>, where x and y denote the Cartesian position of the 
ground contact point, 0 denotes the heading angle and <f> denotes the angle of 
the wheel measured from the vertical. The rolling without slipping condition 
means that 


x — r cos 9(f) = 0 

y — r sin Qcf> = 


0 


(10.107) 
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Fig. 10.5 The Unicycle 


where r is the radius of the wheel. 
1 10.101 ) with q = ( x,y,9,<f > ) and 


These constraints can be written in the form 


Mil = [1 0 0 —rcosd ] 

W 2 = [ 1 0 0 — rsinfl ] 


(10.108) 


Since the dimension of the configuration space is n = 4 and there are two 
constraint equations, we need to find two function gi, <?2 orthogonal to w\, w 2 . 
It is easy to see that 



' 0 ' 


rcosO 


0 


r sin 9 

91 = 

1 

; 92 = 

0 


0 


1 


are both orthogonal to w\ and u >2 • Thus we can write 


(10.109) 


q = g\{q)ui + g2{q)u 2 


( 10 . 110 ) 


where u\ is the turning rate and U2 is the rate of rolling. 


We can now check to see if rolling without slipping constraints on the unicycle 
is holonomic or nonholonomic using Theorem [4] It is easy to show (Problem 10- 


18 1 that the Lie Bracket 


[91,92] 


—r sin 6 
r cos 6 
0 
0 


( 10 . 111 ) 


which is not in the distribution A = span{(/i, g 2 }- Therefore the constraints on 
the unicycle are nonholonomic. We shall see the consequences of this fact in 
the next section when we discuss controllability of driftless systems. 


322 GEOMETRIC NONLINEAR CONTROL 


Example: 10.8 The Kinematic Car. Figure [7 0. 6] shows a simple represen- 
tation of a car, or mobile robot with steerable front wheels. The configuration 



Fig. 10.6 The Kinematic Car 


of the car can be described by q = [x, y, 6, <f] T , where x and y is the point at 
the center of the rear axle, 6 is the heading angle, and <f is the steering angle 
as shown in the figure. The rolling without slipping constraints are found by 
setting the sideways velocity of the front and rear wheels to zero. This leads to 


sin 9 x — cos 9 y = 0 

sin(0 + <f>) x — cos(6* + cj>) y — d cos <f 9 = 0 


( 10 . 112 ) 


which can be written as 

q = <wt,q> =0 
q = < W 2 , q > = 0 


[ sin 9 cos 9 0 0 
[ sin (9 + <f) —cos (9 + (f) —dcoscf 0 


It is thus straightforward to find vectors 



' 0 ' 


cos 9 


0 


sin 9 

9i = 

0 

; 92 = 

i tan (f 


1 


0 


(10.114) 


orthogonal to w \ and W 2 and write the corresponding control system in the 
form (10.106). It is left as an exercise (Problem 19) to show that the above 
constraints are nonholonomic. 


Example: 10.9 A Hopping Robot. Consider the hopping robot in Figure 10.1 
The configuration of this robot is defined by q = (if, I, 9), where 
if = the leg angle 

9 = the body angle 

I = the leg extension 
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During its flight phase the hopping robot ’s angular momentum is conserved. 
Letting I and m denote the body moment of inertia and leg mass, respectively, 
conservation of angular momentum leads to the expression 

I6 + m(e+d) 2 (9 + ip) = 0 (10.115) 

assuming the initial angular momentum is zero. This constraint may be written 
as 


<w,g>=0 (10.116) 

where w = [ m(£+ d) 2 0 I + m(£+ d) 2 ] . Since the dimension of the con- 

figuration space is three and there is one constraint, we need to find two in- 
dependent vectors, g\ and <72 spanning the annihilating distribution A = I}- 1 -, 
where Ll = span { zt; } . It is easy to see that 


' 0 ' 


1 

1 

and g 2 = 

0 

0 

m(£-\-d) 2 

I+m(l-\-d) 2 


(10.117) 


are linearly independent at each point and orthogonal to w. Checking involutiv- 
ity of A we find that 


[51,52] 


0 

0 

—2 Im{£-\-d) 

[I+m(e+d) 2]2 _ 


(10.118) 


Since 53 is not a linear combination of g\ and g -2 it follows that A is not an 
involutive distribution and hence the constraint is nonholonomic. 
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10.7 CHOW’S THEOREM AND CONTROLLABILITY OF DRIFTLESS 
SYSTEMS 

In this section we discuss the controllability properties of driftless systems of 
the form 


x = <71 {x)ui 


g m {x)u r , 


( 10 . 119 ) 


with x £ R”, u £ R m . We assume that the vector fields g\ (x) , . . . g m (x) are 
smooth, complete]^] and linearly independent at each x £ R". 

We have seen previously that if the k < n Pfaffian constraints on the system 
are holonomic then the trajectory of the system lies on a m = n — fc-dimensional 
surface (an integral manifold) found by integrating the constraints. In fact, at 
each x £ R. the tangent space to this manifold is spanned by the vectors gi(x), 
..., g m (x). If we examine Equation ( 10.1191 we see that any instantaneous 
direction in this tangent space, i.e. any linear combination of <?i, • • • , <7m> is 
achievable by suitable choice of the control input terms Ui, i = 1 , . . . , m. Thus 
every point on the manifold may be reached from any other point on the man- 
ifold by suitable control input. However, points not lying on the manifold 
cannot be reached no matter what control input is applied. Thus, for an initial 
condition Xq, only points on the particular integral manifold through Xq are 
reachable. 

What happens if the constraints are nonholonomic? Then no such integral 
manifold of dimension m exists. Thus it might be possible to reach a space 
(manifold) of dimension larger than m by suitable application of the control 
inputs Ui . It turns out that this interesting possibility is true. In fact, by 
suitable combinations of two vector fields g \ and 52 it is possible to move in the 
direction defined by the Lie Bracket [g\ , 52] • If the distribution A = spanjgi , 52 } 
is not involutive, then the Lie Bracket vector field [51,52] defines a direction 
not in the span of 51 and 52 . Therefore, given vector fields 51 , . . . , g m one 
may reach points not only in the span of these vector field but in the span of 
the distribution obtained by augmenting 51, . . . ,g m with various Lie Bracket 
directions. 


Definition 10.11 Involutive Closure of a Distribution 

The involutive closure, A. of a distribution A = span{5i, . . . ,g m } is the small- 
est involutive distribution containing A. In other words, A is an involutive 
distribution such that if Ao is any involutive distribution satisfying A C Aq 
then A C Ao- 

Conceptually, the involutive closure of A can be found by forming larger and 
larger distributions by repeatedly computing Lie Brackets until an involutive 


4 A complete vector field is one for which the solution of the associated differential equation 
exists for all time t 
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distribution is found, i.e. 

A = span{gi, 


■ 9m j [fji ■ 9j\ ; \fJ k : \fji • i/y ] ] , - ■ 


(10.120) 


The involutive closure A in ( 10.120 1 is also called the Control Lie Algebra for 
the driftless control system (10.1191. Intuitively, if dimA = n then all points in 


K" should be reachable from Xo- This is essentially the conclusion of Chow’s 
Theorem. 


Definition 10.12 Controllability 


A driftless system of the form ( 10 . 106 ) is said to be Controllable if, for any xq 
and x\ € R n , there exists a time T > 0 and a control input u = [iti, . . . , u m ] T : 
[0,T] — > R m such that the solution x(t) of ( 10.106 ) satifi.es x(0) = Xq and 
x{T) = X\. 


Given an open set U C R", we let Ry(x o) denote the set of states x such 
that there exists a control u : [0, e] i— > U with x(0) = xq, x(e) = x and x(t) € V 
for 0 < t < e. We set 


Rv,t(Xo) = Uo<e<T Ry (*^o) 
Rv.t is the set of states reachable up to time T > 0. 


(10.121) 


Definition 10.13 We say that the system ( 10.106 ) is Locally Controllable 

at xq if Rv,t(x o ) contains an open neighborhood of Xq for all neighborhoods V 
of xq and T > 0 . 

The next result, known as Chow’s Theorem, gives a sufficient condition 
for the system (10.106) to be locally controllability. 

Theorem 5 The driftless system 


x = g 1 {x)u 1 -\ \-gm{x)u m (10.122) 

is locally controllable at xq € M n */rankA(xo) = n. 

The proof of Chow’s Theorem is beyond the scope of this text. The condition 
rankA(xo) = n is called the Controllability Rank Condition. Note that 
Chow’s theorem tells us when a driftless system is locally controllability but 
does not tell us how to find the control input to steer the system from Xq to xi. 


Example: 10.10 Consider the system on R 3 — {0}. 



x-i 


0 

X = 

X 2 

Ui + 

0 


0 


. Xl 


= gi(x)ui + g 2 (x)u 2 


x 


(10.123) 
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For x 7^ 0 the distribution A = spanjgi, c^} has rank two. It is easy to compute 
the Lie Bracket [<71,52] as 


[51,52] 


-xi 

0 

2:3 


and therefore 


rank [51, 52, [51, 52]] = rank 


X3 0 — xi 

X 2 0 0 

0 Xi X 3 


which has rank three for x 7^ 0. Therefore the system is locally controllable on 
R 3 — {0}. Note that the origin is an equilibrium for the system independent 
of the control input, which is why we must exclude the origin from the above 
analysis. 


Example 10.11 Attitude Control of a Satellite 

Consider a cylindrical satellite equipped with reaction wheels for control as 
shown in Figure \ 10 . 8 \ Suppose we can control the angular velocity about the 





Fig. 10.8 Satellite with Reaction Wheels 

X\, X2, and X3 axes with controls u\, U2, and U3, respectively. The equations of 
motion are then given by 

Cj = LO X U 

with 


Wi 


Ui 

U>2 

u = 

U2 



. U 3 . 


w = 
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Carrying out the above calculation, it is readily shown (Problem 10-20) that 



0 


—u 3 


UJ 2 

UJ = 

w 3 

Ux + 

0 

u 2 + 

-v X 


—u) 2 


UJX 


0 


= gx(u)ux + g 2 (u)u 2 + g 3 (u)u 3 


u 3 


(10.124) 


It is easy to show (Problem 10-21) that the distribution A = span{gx, g 2 , g 3 } 
is involutive of rank 3 on M 3 — {0}. A more interesting property is that the 
satellite is controllable as long as any two of the three reaction wheels are func- 
tioning. The proof of this strictly nonlinear phenomenon is left as a exercise 


(Problem 10-22). o 
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Problems 


10-1 Complete the proof of Lemma 1 1 0 . 1 1 by direct calculation. 


10-2 Show that the function h = z — <p(x, y) satisfies the system (10.201 if <f> is 
a solution of ( 10.13| )-(10.14) and X\, X 2 are defined by (10.18). 


10-3 Show that if h(x,y,z) satisfies (10.20), then, if fy 7^ 0, Equation (10.211 


show that jp = 0 can occur only in the case of the trivial solution h = 0 


can be solved for z as 2 = <; t>{x, y ) where </> satisfies ( 10.13 1-( 10.141. Also 


of (10.20 1. 


10-4 Verify the expressions (10.531 and (10.54). 


10-5 Show that the system below is locally feedback linearizable. 


Xi = 

X‘2 = 


x\ 


%2 

U 


10-6 Derive the equations of motion (10.591 for the single-link manipulator 


with joint elasticity of Figure [1(14 using Lagrange’s equations. 


10-7 Repeat Problem [6] where there is assumed to be viscous friction both on 
the link side and on the motor side of the spring in Figure p 0 . 1 1 


10-8 Perform the calculations necessary to verify (10.65). 


10-9 Derive the system of partial differential Equations (10.68) from the con- 


ditions (10.67 Also verify (10.691. 


10-10 Compute the change of coordinates (10.711. 


10-11 Verify Equations ( 10.73 )-( 10.74 ). 


10-12 Verify Equations (10.781. 


10-13 Design and simulate a linear outer loop control law v for the system 
(10.591 so that the link angle y\(t) follows a desired trajectory yf(t) = 
0f(7) = sin8t. Use various techniques such as pole placement, linear 
quadratic optimal control, etc. (See reference [2] for some ideas.) 

10-14 Consider again a single-link manipulator (either rigid or elastic joint). 
Add to your equations of motion the dynamics of a permanent magnet 
DC-motor. What can you say now about feedback linearizability of the 
system? 


10-15 What happens to the inverse coordinate transformation (10.781 as the 
joint stiffness k — > 00? Give a physical interpretation. Use this to show 
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that the system (10.59) reduces to the equation governing the rigid joint 
manipulator in the limit as k — > oo. 


10-16 Consider the single-link manipulator with elastic joint of Figure [T0.4| but 
suppose that the spring characteristic is nonlinear, that is, suppose that 
the spring force F is given by F = (f>(qi — qz), where <f> is a diffeomor- 
phism. Derive the equations of motion for the system and show that it 
is still feedback linearizable. Carry out the feedback linearizing transfor- 
mation. Specialize the result to the case of a cubic characteristic, i.e. , 
<j> = k(qi — ( 72 ) 3 - The cubic spring characteristic is a more accurate de- 
scription for many manipulators than is the linear spring, especially for 
elasticity arising from gear flexibility. 


10-17 Consider again the single link flexible joint robot given by (10.591 and 
suppose that only the link angle, q±, is measurable. Design an observer 
to estimate the full state vector, x = < 7 i, (ji, q 2 , 92 ) T - 


Hint: Set y = q\ = Cx and show that the system can be written in state 
state as 


x = Ax + bu + <p(y) 

where (j){y) is a nonlinear function depending only on the output y. Then 

a linear observer with output injection can be designed as 

x = Ax + bu + <j){y ) + L(y — Cx) 


10-18 Fill in the details in Example |10.7| showing that the constraints are non- 
holonomic. 

10-19 Fill in the details in Example |10.8| necessary to derive the vector fields 
< 7 i and 772 and show that the constraints are nonholonomic. 

10-20 Carry out the calculations necessary to show that the equations of motion 


for the satellite with reaction wheels is given by Equation 10.124 


10-21 Show that the distribution A = span(< 7 i, 772 , 773 ) in for the satellite model 
(10.124) is involutive of rank 3. 

10-22 Using Chow’s Theorem, show that the satellite with reaction wheels 
(10.124) is controllable as long as any two of the three reaction wheels 
are functioning. 



11 

COMPUTER VISION 


If a robot is to interact with its environment, then the robot must be able 
to sense its environment. Computer vision is one of the most powerful sensing 
modalities that currently exist. Therefore, in this chapter we present a number 
of basic concepts from the field of computer vision. It is not our intention here 
to cover the now vast field of computer vision. Rather, we aim to present a num- 
ber of basic techniques that are applicable to the highly constrained problems 
that often present themselves in industrial applications. The material in this 
chapter, when combined with the material of previous chapters, should enable 
the reader to implement a rudimentary vision-based robotic manipulation sys- 
tem. For example, using techniques presented in this chapter, one could design 
a system that locates objects on a conveyor belt, and determines the positions 
and orientations of those objects. This information could then be used in con- 
junction with the inverse kinematic solution for the robot, along with various 
coordinate transformations, to command the robot to grasp these objects. 

We begin by examining the geometry of the image formation process. This 
will provide us with the fundamental geometric relationships between objects 
in the world and their projections in an image. We then describe a calibra- 
tion process that can be used to determine the values for the various camera 
parameters that appear in these relationships. We then consider image segmen- 
tation, the problem of dividing the image into distinct regions that correspond 
to the background and to objects in the scene. When there are multiple objects 
in the scene, it is often useful to deal with them individually; therefore, we 
next present an approach to component labelling. Finally, we describe how to 
compute the positions and orientations of objects in the image. 
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11.1 THE GEOMETRY OF IMAGE FORMATION 

A digital image is a two-dimensional array of pixels that is formed by focusing 
light onto a two-dimensional array of sensing elements. A lens with focal length 
A is used to focus the light onto the sensing array, which is often composed of 
CCD (charge-coupled device) sensors. The lens and sensing array are packaged 
together in a camera, which is connected to a digitizer or frame grabber. In the 
case of analog cameras, the digitizer converts the analog video signal that is 
output by the camera into discrete values that are then transferred to the pixel 
array by the frame grabber. In the case of digital cameras, a frame grabber 
merely transfers the digital data from the camera to the pixel array. Associated 
with each pixel in the digital image is a gray level value, typically between 0 
and 255, which encodes the intensity of the light incident on the corresponding 
sensing element. 

In robotics applications, it is often sufficient to consider only the geometric 
aspects of image formation. Therefore in this section we will describe only the 
geometry of the image formation process. We will not deal with the photometric 
aspects of image formation (e.g., we will not address issues related to depth of 
field, lens models, or radiometry). 

We will begin the section by assigning a coordinate frame to the imaging 
system. We then discuss the popular pinhole model of image formation, and 
derive the corresponding equations that relate the coordinates of a point in the 
world to its image coordinates. Finally, we describe camera calibration, the 
process by which all of the relevant parameters associated with the imaging 
process can be determined. 


11.1.1 The Camera Coordinate Frame 


In order to simplify many of the equations of this chapter, it often will be useful 
to express the coordinates of objects relative to a camera centered coordinate 
frame. For this purpose, we define the camera coordinate frame as follows. 
Define the image plane, 7 r, as the plane that contains the sensing array. The 
axes x c and y c form a basis for the image plane, and are typically taken to be 
parallel to the horizontal and vertical axes (respectively) of the image. The 
axis z c is perpendicular to the image plane and aligned with the optic axis of 
the lens (i.e., it passes through the focal center of the lens). The origin of the 
camera frame is located at a distance A behind the image plane. This point 
is also referred to as the center of projection. The point at which the optical 
axis intersects the image plane is known as the principal point. This coordinate 


frame is illustrated in Figure 11.1 


With this assignment of the camera frame, any point that is contained in the 
image plane will have coordinates (u,v,X). Thus, we can use (u,v) to parame- 
terize the image plane, and we will refer to (u, v) as image plane coordinates. 
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Y 



Fig. 11.1 Camera coordinate frame. 


11.1.2 Perspective Projection 


The image formation process is often modeled by the pinhole lens approxima- 
tion. With this approximation, the lens is considered to be an ideal pinhole, and 
the pinhole is located at the focal center of the lentQ Light rays pass through 
this pinhole, and intersect the image plane. 

Let P be a point in the world with coordinates x, y, z (relative to the camera 
frame). Let p denote the projection of P onto the image plane with coordinates 
(u,v, A). Under the pinhole assumption, P, p and the origin of the camera 
frame will be collinear. This can is illustrated in Figure 11.1 Thus, for some 
unknown positive k we have 


*(i) _ (i) <1L1) 

which can be rewritten as the system of equations: 


kx = 

u, 

(11.2) 

ky = 

v, 

(11.3) 

kz = 

A. 

(11.4) 


^■Note that in our mathematical model, illustrated in Figure [ll.l| we have placed the pinhole 
behind the image plane in order to simplify the model. 
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This gives k = X/z, which can be substituted into (11.21 and (11.31 to obtain 

(11.5) 

These are the well-known equations for perspective projection. 


\ x 

u = A — , 
z 


,y 

v = A-. 
z 


11.1.3 The Image Plane and the Sensor Array 

As described above, the image is a discrete array of gray level values. We will 
denote the row and column indices for a pixel by the coordinates (r, c) . In order 
to relate digital images to the 3D world, we must determine the relationship 
between the image plane coordinates, (u,v), and indices into the pixel array of 
pixels, (r, c). 

We typically define the origin of this pixel array to be located at a corner of 
the image (rather than the center of the image) . Let the pixel array coordinates 
of the pixel that contains the principal point be given by (o r , o c ). In general, the 
sensing elements in the camera will not be of unit size, nor will they necessarily 
be square. Denote the s x and s y the horizontal and vertical dimensions (respec- 
tively) of a pixel. Finally, it is often the case that the horizontal and vertical 
axes of the pixel array coordinate system point in opposite directions from the 
horizontal and vertical axes of the camera coordinate fram(0 Combining these, 
we obtain the following relationship between image plane coordinates and pixel 
array coordinates, 


which gives, 


u t \ 

= {r- Or), 

v t \ 

= (c- o c ). 

(11.6) 

s x 

Sy 

0 

1 

9 

cc 

1 

II 

3 

v = ~ s y( c - Oc)- 

(11.7) 


Note that the coordinates (r, c) will be integers, since they are the discrete 
indices into an array that is stored in computer memory. Therefore, it is not 
possible to obtain the exact image plane coordinates for a point from the (r, c) 
coordinates. 


11.2 CAMERA CALIBRATION 

The objective of camera calibration is to determine all of the parameters that 
are necessary to predict the image pixel coordinates (r, c) of the projection of 
a point in the camera’s field of view, given that the coordinates of that point 
with respect to the world coordinate frame are know. In other words, given the 
coordinates of P relative to the world coordinate frame, after we have calibrated 


2 This is an artifact of our choice to place the center of projection behind the image plane. 
The directions of the pixel array axes may vary, depending on the frame grabber. 
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the camera we will be able to predict (r, c), the image pixel coordinates for the 
projection of this point. 


11.2.1 Extrinsic Camera Parameters 

To this point, in our derivations of the equations for perspective projection, 
we have dealt only with coordinates expressed relative to the camera frame. 
In typical robotics applications, tasks will be expressed in terms of the world 
coordinate frame, and it will therefore be necessary to perform coordinate trans- 
formations. If we know the position and orientation of the camera frame relative 
to the world coordinate frame we have 

x w = R w c x c + 0“ (11.8) 

or, if we know x w and wish to solve for x c , 

x c = R c w (x w - O?) (11.9) 

In the remainder of this section, to simplify notation we will define 

R = R c w , T = - R c w O (11.10) 

Thus, 

x c = Rx w + T (11.11) 

Cameras are typically mounted on tripods, or on mechanical positioning 
units. In the latter case, a popular configuration is the pan/tilt head. A pan/tilt 
head has two degrees of freedom: a rotation about the world 2 axis and a 
rotation about the pan/tilt head’s x axis. These two degrees of freedom are 
analogous to the those of a human head, which can easily look up or down, and 
can turn from side to side. In this case, the rotation matrix R is given by 

R = R z , S Rx, a, (11-12) 

where 9 is the pan angle and a is the tilt angle. More precisely, 9 is the angle 
between the world x-axis and the camera rr-axis, about the world 2 -axis, while a 
is the angle between the world 2 -axis and the camera 2 -axis, about the camera 
a:- axis. 

11.2.2 Intrinsic Camera Parameters 

Using the pinhole model, we obtained the following equations that map the 
coordinates of a point expressed with respect to the camera frame to the cor- 
responding pixel coordinates: 

u , v x y 

r= 1 - o r , c= 1 - o c , u = A— v = a-. (11. Id) 

s x s y z z 
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These equations can be combined to give 

A x 

r = bo r , c 

s x z 


A y 

Sy z 


+ O c , 


( 11 . 14 ) 


Thus, once we know the values of the parameters A, s x ,o r , s yi o c we can deter- 
mine (r, c) from ( x,y,z ), where ( x,y,z ) are coordinates relative to the camera 
frame. In fact, we don’t need to know all of A, s x , s y ; it is sufficient to know the 
ratios 

/* = -- /„ = --• ( 11 - 15 ) 

S x Sy 

These parameters, f x ,o r , f y ,o c are known as the intrinsic parameters of the 
camera. They are constant for a given camera, and do not change when the 
camera moves. 


11.2.3 Determining the Camera Parameters 

The task of camera calibration is to determine the intrinsic and extrinsic pa- 
rameters of the camera. We will proceed by first determining the parameters 
associated with the image center, and then solving for the remaining parame- 
ters. 

Of all the camera parameters, o r ,o c (the image pixel coordinates of the 
principal point) are the easiest to determine. This can be done by using the 
idea of vanishing points. Although a full treatment of vanishing points is beyond 
the scope of this text, the idea is simple: a set of parallel lines in the world will 
project onto image lines that intersect at a single point, and this intersection 
point is known as a vanishing point. The vanishing points for three mutually 
orthogonal sets of lines in the world will define a triangle in the image. The 
orthocenter of this triangle (i.e., the point at which the three altitudes intersect) 
is the image principal point (a proof of this is beyond the scope of this text). 
Thus, a simple way to compute the principal point is to position a cube in 
the workspace, find the edges of the cube in the image (this will produce the 
three sets of mutually orthogonal parallel lines), compute the intersections of 
the image lines that correspond to each set of parallel lines in the world, and 
determine the orthocenter for the resulting triangle. 

Once we know the principal point, we proceed to determine the remaining 
camera parameters. This is done by constructing a linear system of equations in 
terms of the known coordinates of points in the world and the pixel coordinates 
of their projections in the image. The unknowns in this system are the camera 
parameters. Thus, the first step in this stage of calibration is to acquire a 
data set of the form rq, c\, x\, yi, zi, r 2 , c 2 , £2, Vi, Z2, ■ ■ • rjv, cjv, Xn, Vn, zn, in 
which ri,Ci are the image pixel coordinates of the projection of a point in the 
world with coordinates Xi,yi,Zi relative to the world coordinate frame. This 
acquisition is often done manually, e.g., by having a robot move a small bright 
light to known x, y, z coordinates in the world, and then hand selecting the 
corresponding image point. 
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Once we have acquired the data set, we proceed to set up the linear system 
of equations. The extrinsic parameters of the camera are given by 


ru 

T \2 

T 1,3 


' T x ' 

T 21 

T 22 

T23 

, T = 

Ty 

. r 31 

T 32 

X 33 _ 


T z 


(11.16) 


With respect to the camera frame, the coordinates of a point in the world are 
thus given by 


x c = mi + r 12 y + r 13 z + T x 


y c = r 2 ix + r 22 y + r 23 z + T y 
z c = r 31 x + r 32 y + r 33 z + T z . 


Combining this with 
r — o r 

c- o c 


(11.14) we obtain 



~fx 



-fy 


r\\X + r\ 2 y + r 33 z + T x 
T31X + r 32 y + r 33 z + T z 
r2ix + r 22 y + r 23 z + T y 
T31X + r 32 y + r 33 z + T z ' 


(11.17) 

(11.18) 
(11.19) 


(11.20) 

( 11 . 21 ) 


Since we know the coordinates of the principal point, we an simplify these 
equations by using the simple coordinate transformation 


r 


r - o r , 


(11.22) 


We now write the two transformed projection equations as functions of the 
unknown variables: rij,T x ,T y ,T z , f x , f y . This is done by solving each of these 
equations for z c , and setting the resulting equations to be equal to one another. 
In particular, for the data points Ty, Cj, Xi , yi, Zi we have 


rif y (r 2 \Xi + r 22 yi + r 23 Zi + T y ) = Cif x (ruXi + r X2 yi + r 13 z. t + T x ). (11.23) 
We define a = f x / f y and rewrite this as: 

rir 2 \Xi + rir 22 yi + rir 23 Zi + r{Ty - aCirnXi - aCir 12 yi - aCir 13 Zi - ac{T x = 0. 

(11.24) 

We can combine the N such equations into the matrix equation 


rix 1 

T2X2 

r iVi 

T2V2 

r\Z\ 

T2Z2 

T2 

-C1X1 

~c 2 x 2 

-Cl 2/1 
~c 2 y 2 

-C1Z1 

-c 2 z 2 

r N x N 

T NyN 

r N z N 

tn 

C N X N 

- cnVn 

— CnZn 



T21 


T22 


X23 


Ty 


arn 


otr 12 


ar 13 


aT x 


(11.25) 
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This is an equation of the form Ax = 0. As such, if x = [x \ , • • • ,xs] T is a 
solution, for (11.251 we only know that this solution is some scalar multiple of 
the desired solution, x , i.e., 

x = k[r 2 i,r 22 ,r 23 ,T y , ar 1± , ar 12 , ar 33 , aT x ] T , (11.26) 

in which k is an unknown scale factor. 

In order to solve for the true values of the camera parameters, we can exploit 
constraints that arise from the fact that R is a rotation matrix. In particular, 


(5? + ** + *§)* = (k 2 (r 2 21 + r| 2 + r 23 )) 2 = |fc|, 


(11.27) 


and likewise 


(x 5 + x 6 + x 7 ) 2 — (a k (r 21 + r 22 + r 23 )) 2 — a\k\ 


(11.28) 


(note that by definition, a > 0). 

Our next task is to determine the sign of k. Using equations (11.141 we 
see that r x x c < 0 (recall that we have used the coordinate transformation 
r <— r — o r ). Therefore, to determine the sign of k , we first assume that k > 0. 
If r(r \\x + ri 2 y + r\ 3 z + T x ) < 0, then we know that we have made the right 
choice and k > 0; otherwise, we know that k < 0. 

At this point, we know the values for k,a,r 2 i,r 22 ,r 23 ,ru,ri 2 ,ri 3 ,T x ,TY, 
and all that remains is to determine T z ,f x ,f y . Since a = f x /fy, we need only 
determine T z and f x . Returning again to the projection equations, we can write 


t x c _ j. rux + r V2 y + r 13 z + T x 

T JX J X rf, 

z c r 31 x + r 32 y + r 33 z + T z 


(11.29) 


Using an approach similar to that used above to solve for the first eight param- 
eters, we can write this as the linear system 

r(r 3 ix + r 32 y + r 33 z + T z ) = -f x (ru x + r 12 y + r 13 z + T x ) (11.30) 

which can easily be solved for Tz and f x . 


11.3 SEGMENTATION BY THRESHOLDING 

Segmentation is the process by which an image is divided into meaningful com- 
ponents. Segmentation has been the topic of computer vision research since 
its earliest days, and the approaches to segmentation are far too numerous to 
survey here. These approaches are sometimes concerned with finding features 
in an image (e.g., edges), and sometimes concerned with partitioning the im- 
age into homogeneous regions (region-based segmentation). In many practical 
applications, the goal of segmentation is merely to divide the image into two 
regions: one region that corresponds to an object in the scene, and one region 
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For i = 0 to N — 1 


H[i] <- 0 


For r = 0 to N rows — 1 


rows 


For c = 0 to N co i s - 1 
Index <— Image(r, c) 

H [Index] <— H [Index] + 1 


Fig. 11.2 Pseudo-code to compute an image histogram. 


that corresponds to the background. In many industrial applications, this seg- 
mentation can be accomplished by a straight-forward thresholding approach. 
Pixels whose gray level is greater than the threshold are considered to belong 
to the object, and pixels whose gray level is less than or equal to the threshold 
are considered to belong to the background. 

In this section we will describe an algorithm that automatically selects a 
threshold. This basic idea behind the algorithm is that the pixels should be 
divided into two groups (background and object), and that the intensities of 
the pixels in a particular group should all be fairly similar. To quantify this 
idea, we will use some standard techniques from statistics. Thus, we begin the 
section with a quick review of the necessary concepts from statistics and then 
proceed to describe the threshold selection algorithm. 

11.3.1 A Brief Statistics Review 

Many approaches to segmentation exploit statistical information contained in 
the image. In this section, we briefly review some of the more useful statistical 
concepts that are used by segmentation algorithms. 

The basic premise for most of these statistical concepts is that the gray level 
value associated with a pixel in an image is a random variable that takes on 
values in the set {0, 1, • • • N — 1}. Let P(z) denote the probability that a pixel 
has gray level value z. In general, we will not know this probability, but we 
can estimate it with the use of a histogram. A histogram is an array, H , that 
encodes the number of occurrences of each gray level value. In particular, the 
entry H[z\ is the number of times gray level value z occurs in the image. Thus, 
0 < H[z\ < N rows x N co i s for all 2 . An algorithm to compute the histogram for 
an image is shown in figure |11 .2 

Given the histogram for the image, we estimate the probability that a pixel 
will have gray level z by 



(11.31) 


Thus, the image histogram is a scaled version of our approximation of P. 
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Given P, we can compute the average , or mean value of the gray level values 
in the image. We denote the mean by /r, and compute it by 


N-l 

H='£zP(z). (11.32) 

.-=o 

In many applications, the image will consist of one or more objects against 
some background. In such applications, it is often useful to compute the mean 
for each object in the image, and also for the background. This computation 
can be effected by constructing individual histogram arrays for each object, and 
for the background, in the image. If we denote by Hi the histogram for the i th 
object in the image (where i = 0 denotes the background) , the mean for the i th 
object is given by 


^ —^2 z „jv— i jr r i ’ (11.33) 

s= o E z=0 

which is a straightforward generalization of 

Hj[z\ 

Ef=o l HA 

is in fact an estimate of the probability that a pixel will have gray level value 
z given that the pixel is a part of object i in the image. For this reason, /q is 
sometimes called a conditional mean. 

The mean conveys useful, but very limited information about the distribution 
of grey level values in an image. For example, if half or the pixels have gray value 
127 and the remaining half have gray value 128, the mean will be /j, = 127.5. 
Likewise, if half or the pixels have gray value 255 and the remaining half have 
gray value 0, the mean will be fi = 127.5. Clearly these two images are very 
different, but this difference is not reflected by the mean. One way to capture 
this difference is to compute the the average deviation of gray values from the 
mean. This average would be small for the first example, and large for the 
second. We could, for example, use the average value of \z — /i|; however, it 
will be more convenient mathematically to use the square of this value instead. 
The resulting quantity is known as the variance , which is defined by 

N-l 

a 2 = ^(^/r) 2 P( Z ). (11.34) 

z= 0 

As with the mean, we can also compute the conditional variance, of for each 
object in the image 


( 11.32 1. The term 


N-l 




Hi[z] 


z = 0 


£f=o 'HA 


(11.35) 
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11.3.2 Automatic Threshold Selection 


We are now prepared to develop an automatic threshold selection algorithm. 
We will assume that the image consists of an object and a background, and 
that the background pixels have gray level values less than or equal to some 
threshold while the object pixels are above the threshold. Thus, for a given 
threshold value, Zt, we divide the image pixels into two groups: those pixels 
with gray level value z < Zt, and those pixels with gray level value z > Zt- We 
can compute the means and variance for each of these groups using the equations 
of Section ra Clearly, the conditional means and variances depend on the 
choice of z t , since it is the choice of z t that determines which pixels will belong 
to each of the two groups. The approach that we take in this section is to 
determine the value for Zt that minimizes a function of the variances of these 
two groups of pixels. 

In this section, it will be convenient to rewrite the conditional means and 
variances in terms of the pixels in the two groups. To do this, we define qi{zt) as 
the probability that a pixel in the image will belong to group i for a particular 
choice of threshold, z t . Since all pixels in the background have gray value less 
than or equal to Zt and all pixels in the object have gray value greater than Zt, 
we can define qi{z t ) for * = 0,1 by 


E?=o H[z 


Ef="l +1 H[z 


go(*) = ,^ z= r» i ; j ^ 9i (*)= " 2=zt+1 


(-^ rows Ncols) 


We now rewrite (|11.33 ) as 
HAz] 


N-l 


N—l 


ft 


= £ 


= £ 


{N rows Y N co ls) 

Hi [z\ J (N rovJS T Neals') 


(11.36) 


=o Ez=o Hi[z\ z=0 E z =o Hi[z\/{N rows x N co is) 


z= 0 0 1 11 2=0 Zjz= 0 

Using again the fact that the two pixel groups are defined by the threshold z t) 
we have 


Hnlz] 


= P(z) 

(N rows X Neals') qo(Zt) 


, z < Zt 


and 


HM 


P{z) 


( N rows x Ncois) qi(zt) 

(11.37) 

Thus, we can write the conditional means for the two groups as 


0 > Zt- 


{ ^ v'' P{ z ) 

MO {Zt) = Vz-TT: 


N-l 


Mi(^) = 


. H z ) 

' qi{z t y 


(11.38) 


Z=Z t +l 

Similarly, we can write the equations for the conditional variances by 


^0 (~t) = J2( z ~ 


Kz t ) 


N P(z ' I 

£ {z-^zt)) 2 ^- (11.39) 

z=z f + l 1l\ z t) 


We now turn to the selection of Zt- If nothing is known about the true values 
of Hi or <jf, how can we determine the optimal value of Zf? To answer this 
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question, recall that the variance is a measure of the average deviation of pixel 
intensities from the mean. Thus, if we make a good choice for z t , we would 
expect that the variances crj(z t ) would be small. This reflects the assumption 
that pixels belonging to the object will be clustered closely about /ii, pixels 
belonging to the background will be clustered closely about /j,q. We could, 
therefore, select the value of z t that minimizes the sum of these two variances. 
However, it is unlikely that the object and background will occupy the same 
number of pixels in the image; merely adding the variances gives both regions 
equal importance. A more reasonable approach is to weight the two variances 
by the probability that a pixel will belong to the corresponding region, 


= Qo(zt)<r$(zt) + qi{z t )al{z t ). (11.40) 


The value erj is known as the vuithin-group variance. The approach that we will 
take is to minimize this within-group variance. 

At this point we could implement a threshold selection algorithm. The naive 
approach would be to simply iterate over all possible values of z t and select 
the one for which <r^( Zt ) is smallest. Such an algorithm performs an enormous 
amount of calculation, much of which is identical for different candidate values of 
the threshold. For example, most of the calculations required to compute cr^,{z t ) 
are also required to compute cr^(z t + 1); the required summations change only 
slightly from one iteration to the next. Therefore, we now turn our attention 
to an efficient algorithm. 

To develop an efficient algorithm, we take two steps. First, we will derive 
the between-group variance, which depends on the within-group variance 
and the variance over the entire image. The between-group variance is a bit 
simpler to deal with than the within-group variance, and we will show that 
maximizing the between-group variance is equivalent to minimizing the within- 
group variance. Then, we will derive a recursive formulation for the between- 
group variance that lends itself to an efficient implementation. 

To derive the between-group variance, we begin by expanding the equation 
for the total variance of the image, and then simplifying and grouping terms. 
The variance of the gray level values in the image is given by (11.341, which 
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can be rewritten as 

N-l 

<T 2 = 5>-m) 2 P(2) 

2=0 

= f>-M) 2 P(2)+ (^-M) 2 ^) 

2=0 z=z± 1 

N-l 

= ^(2-/i0 + M0-/^) 2 -PW+ 51 - Ml + Ml - V) 2 P{z) 

2=0 2=2t + l 

Zt 

= 53^ - Mo) 2 + 2 ( z - Mo)(mo - m) + (Mo - m) 2 ]^W 
2=0 

N-l 

+ 53 ~ Ml) 2 + 2 ( 2: ~Mi)(Mi-M) + (m-M) 2 ]-P(2)- (11-41) 

,~=z t + l 


Note that the we have not explicitly noted the dependence on zt here. In 
the remainder of this section, to simplify notation, we will refer to the group 
probabilities and conditional means and variances as qi, fa, and a 2 , without 
explicitly noting the dependence on z t . This last expression (11.411 can be 
further simplified by examining the cross-terms 


53( z - fa)(fa - Li)P(z) 


53 zfaP{z) - 53 zhp(z) - 53 tfp(z) + 53 fafiP(z) 

fa 53 zP(z) -n 53 zP(z) -M? 53 p (~) + m*m 53 

fa(faQi) - v(mqi) - i4<ii + MiM% 

0, 


in which the summations are taken for z from 0 to z t for the background pixels 
(i.e., i = 0) and z from z t + 1 to N — 1 for the object pixels (i.e., i = 1). 
Therefore, we can simplify (11.41 1 to obtain 


z t N-l 

(j“ = 53[(z - mo) 2 + (Mo - M) 2 TO + 53 [( z - Mi) 2 + (Mi — L l ) 2 }P( z ) 

2 = 0 2=2t + l 

= Qo<To + 9o(Mo - m) 2 + Qi<Ti + 9i(Mi - m) 2 
= {qo^o + <7icr 2 } + {g 0 (Mo - m) 2 + 9i(Mi - m) 2 } 

= cr 2 w + a 2 (H-42) 

in which 

0-2 = 9o(Mo - M) 2 + 9i(Mi - M) 2 - (11.43) 

Since cr 2 does not depend on the threshold value (i.e., it is constant for a spe- 
cific image), minimizing a 2 , is equivalent to maximizing er 2 . This is preferable 
because a 2 is a function only of the qi and fa, and is thus simpler to compute 
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than cr^, which depends also on the of. In fact, by expanding the squares in 
(11.431, using the facts that qi = 1 — q 0 and n = qi^o + qiHi, we obtain 

°b = 9o(l — 9 o)(mo - Mi) 2 - (11.44) 


The simplest algorithm to maximize of is to iterate over all possible threshold 
values, and select the one that maximizes of. However, as discussed above, 
such an algorithm performs many redundant calculations, since most of the 
calculations required to compute crjj(zt) are also required to compute cr%(zt + l). 
Therefore, we now turn our attention to an efficient algorithm that maximizes 
of (z t ). The basic idea for the efficient algorithm is to re-use the computations 
needed for cr^(zt) when computing of (^ + 1). In particular, we will derive 
expressions for the necessary terms at iteration z t + 1 in terms of expressions 
that were computed at iteration z t . We begin with the group probabilities, and 
determine the recursive expression for q$ as 


Zf \~ 1 


qo(z t + 1) — P{z) — P(z t + 1) + P{z) — P(z t + 1) + qa(zt). 


2=0 


2=0 


(11.45) 


In this expression, P(zt + 1) can be obtained directly from the histogram ar- 
ray, and qo(zt) is directly available because it was computed on the previous 
iteration of the algorithm. Thus, given the results from iteration Zt, very little 
computation is required to compute the value for go at iteration z t + 1. 

For the conditional mean p.o(~t) we have 


Mo {zt + 1) 


2 t + l 

E 


P(z) 


9o (z t + 1) 


2=0 


(zt + 1 )P(zt + 1) P(z) 

+ 2^ z - 


Qo (zt + 1) 


2=0 


9o (zt + 1) 


(zt + 1 )P(zt + 1) + qo(zt) 
Qo (zt + 1 ) 

(zt + 1 )P(z t + 1) 

Qo (zt + 1) 


y z p 

9o ( z t + !) ^ Qo(z t ) 

Qo{z t ) , , 


(11.46) 

(11.47) 

(11.48) 

(11.49) 


Again, all of the quantities in this expression are available either from the 
histogram, or as the results of calculations performed at iteration z t of the 
algorithm. 

To compute fi\{zt + 1), we use the relationship /i = <?oMo + giMP which can 
be easily obtained using (11.321 and (|11 .38 ) . Thus, we have 


+ 1) = " - ^ + 1)w fr + 11 = a - ^ +, 1)> ‘° (2 ‘ + 11 . (11.50) 

Qi(z t + 1) 1 — Qo( z t + 1) 


We are now equipped to construct a highly efficient algorithm that automat- 
ically selects a threshold value that minimizes the within-group variance. This 
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Fig- 

the 


11.4 (a) Histogram for the image shown in 11.3 1 (b) Within-group variance for 

image shown in|11.3(t 


algorithm simply iterates from 0 to N — 1 (where N is the total number of gray 
level values), computing qo, /j,q, /ii and <t% at each iteration using the recur- 
sive formulations given in (11.451, ( 11.49| ), (11.50) and ( 11.44 1. The algorithm 
returns the value of Zt for which cr£ is largest. Figure 11.3 shows a grey level 
image and the binary, thresholded image that results from the application of 
this algorithm. F igure [ll.4| shows the histogram and within-group variance for 
the grey level image. 
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11.4 CONNECTED COMPONENTS 

It is often the case that multiple objects will be present in a single image. When 
this occurs, after thresholding there will be multiple connected components 
with gray level values that are above the threshold. In this section, we will 
first make precise the notion of a connected component, and then describe an 
algorithm that assigns a unique label to each connected component, i.e. , all 
pixels within a single connected component have the same label, but pixels in 
different connected components have different labels. 

In order to define what is meant by a connected component, it is first neces- 
sary to define what is meant by connectivity. For our purposes, it is sufficient 
to say that a pixel, A, with image pixel coordinates (r, c) is adjacent to four 
pixels, those with image pixel coordinates (r — l,c), (r + l,c), (r, c + 1), and 
(r, c — 1). In other words, each image pixel A (except those at the edges of 
the image) has four neighbors: the pixel directly above, directly below, directly 
to the right and directly to the left of pixel A. This relationship is sometimes 
referred to as 4~ connectivity , and we say that two pixels are 4-connected if they 
are adjacent by this definition. If we expand the definition of adjacency to in- 
clude those pixels that are diagonally adjacent (i.e., the pixels with coordinates 
(r — l,c— 1), (r — l,c+ 1), (r + l,c— 1), and (r + l,c+ 1)), then we say that 
adjacent pixels are 8-connected. In this text, we will consider only the case of 
4-connectivity. 

A connected component is a collection of pixels, S, such that for any two 
pixels, say P and P' , in S, there is a 4-connected path between them and this 
path is contained in S. Intuitively, this definition means that it is possible 
to move from P to P 1 by “taking steps” only to adjacent pixels without ever 
leaving the region S. The purpose of a component labeling algorithm is to 
assign a unique label to each such S. 

There are many component labeling algorithms that have been developed 
over the years. Here, we describe a simple algorithm that requires two passes 
over the image. This algorithm performs two raster scans of the image (note: 
a raster scan visits each pixel in the image by traversing from left to right, 
top to bottom, in the same way that one reads a page of text). On the first 
raster scan, when an object pixel P , (i.e., a pixel whose gray level is above the 
threshold value), is encountered, its previously visited neighbors (i.e., the pixel 
immediately above and the pixel immediately to the left of P) are examined, 
and if they have gray value that is below the threshold (i.e., they are background 
pixels) , a new label is given to P. This is done by using a global counter that is 
initialized to zero, and is incremented each time a new label is needed. If either 
of these two neighbors have already received labels, then P is given the smaller 
of these, and in the case when both of the neighbors have received labels, an 
equivalence is noted between those two labels. For example, in Figure |11.5 
after the first raster scan labels (2,3,4) are noted as equivalent. In the second 
raster scan, each pixel’s label is replaced by the smallest label to which it is 
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(c) 


(d) 


Fig. 11.5 The image in (a) is a simple binary image. Background pixels are denoted by 
0 and object pixels are denoted by X. Image (b) shows the assigned labels after the first 
raster scan. In image (c), an X denotes those pixels at which an equivalence is noted 
during the first raster scan. Image (d) shows the final component labelled image. 


equivalent. Thus, in the example of Figure |11.5[ at the end of the second raster 
scan labels 3 and 4 have been replaced by the label 2. 

After this algorithm has assigned labels to the components in the image, 
it is not necessarily the case that the labels will be the consecutive integers 
(1, 2, • • • ). Therefore, a second stage of processing is sometimes used to relabel 
the components to achieve this. In other cases, it is desirable to give each 
component a label that is very different from the labels of the other components. 
For example, if the component labelled image is to be displayed, it is useful to 
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Fig. 11.6 The image of Figure [TT3] after connected components have been labelled. 


increase the contrast, so that distinct components will actually appear distinct 
in the image (a component with the label 2 will appear almost indistinguishable 
from a component with label 3 if the component labels are used as pixel gray 
values in the displayed component labelled image) . The results of applying this 
process to the image in Figure [T 1. 3 1 are shown in Figure [Tl.6| 

When there are multiple connected object components, it is often useful to 
process each component individually. For example, we might like to compute 
the sizes of the various components. For this purpose, it is useful to introduce 
the indicator function for a component. The indicator function for component 
i, denoted by 2), is a function that takes on the value 1 for pixels that are 
contained in component i, and the value 0 for all other pixels: 


{ 1 : pixel r, c is contained in component i 

0 : otherwise 


(11.51) 


We will make use of the indicator function below, when we discuss computing 
statistics associated with the various objects in the image. 


11.5 POSITION AND ORIENTATION 

The ultimate goal of a robotic system is to manipulate objects in the world. 
In order to achieve this, it is necessary to know the positions and orientations 
of the objects that are to be manipulated. In this section, we address the 
problem of determining the position and orientation of objects in the image. If 
the camera has been calibrated, it is then possible to use these image position 
and orientations to infer the 3D positions and orientations of the objects. In 
general, this problem of inferring the 3D position and orientation from image 
measurements can be a difficult problem; however, for many cases that are 
faced by industrial robots we an obtain adequate solutions. For example, when 
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grasping parts from a conveyor belt, the depth, z, is fixed, and the perspective 
projection equations can be inverted if z is known. 

We begin the section with a general discussion of moments, since moments 
will be used in the computation of both position and orientation of objects in 
the image. 

11.5.1 Moments 

Moments are functions defined on the image that can be used to summarize 
various aspects of the shape and size of objects in the image. The i,j moment 
for the k th object, denoted by m.y(fc), is defined by 

rriij(k) = ^ ]rVI t (r,c ). (11.52) 

r,c 

From this definition, it is evident that moo is merely number of pixels in the 
object. The order of a moment is defined to be the sum i + j- The first order 
moments are of particular interest when computing the centroid of an object, 
and they are given by 

mio(fc) = y~V:r fc (r,c), m 0 i(fc) = ^cT fe (r,c). (11.53) 

r,c r,c 

It is often useful to compute moments with respect to the object center of 
mass. By doing so, we obtain characteristics that are invariant with respect to 
translation of the object. These moments are called central moments. The i,j 
central moment for the k th object is defined by 

Cij(k) = Y( r - r) l {c-c) 3 Ik{r,c), (11.54) 

r,c 

in which (f, c) are the coordinates for the center of mass, or centroid, of the 
object. 


11.5.2 The Centroid of an Object 


It is convenient to define the position of an object to be the object’s center of 
mass or centroid. By definition, the center of mass of an object is that point 
(r, c) such that, if all of the object’s mass were concentrated at (f, c) the first 
moments would not change. Thus, we have 


Y nZi{r, c) = Y c ) 


= E r,c rI i( r ’ C ) 
Er/ifn C ) 


mi 0 (i) 

m 00 {i) 


(11.55) 


Y CiZi(r, c) = Y cIi ( r ’ c ) 


^ 7 . c cXj(r, c) 

Er/'fn c ) 


TOoi(*) 

TOoo(i) 


(.11.56) 


Figure 

Figure 


11.7 

TO 


shows the centroids for the connected components of the image of 
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Fig. 11.7 The segmented, component-labeled image of figure |TT~TT| showing the centroids 
and orientation of each component. 


11.5.3 The Orientation of an Object 

We will define the orientation of an object in the image to be the orientation 
of an axis that passes through the object such that the second moment of the 
object about that axis is minimal. This axis is merely the two-dimensional 
equivalent of the axis of least inertia. 

For a given line in the image, the second moment of the object about that 
line is given by 


£ = ^d 2 (r,c)T(r,c) (11.57) 

r,c 

in which d(r, c) is the minimum distance from the pixel with coordinates (r, c) 
to the line. Our task is to minimize C with respect to all possible lines in the 
image plane. To do this, we will use the p, 6 parameterization of lines, and 
compute the partial derivatives of C with respect to p and 0. We find the 
minimum by setting these partial derivatives to zero. 

With the p , 6 parameterization, a line consists of all those points x, y that 
satisfy 

x cos 9 -\- y sin 9 — p = 0. (11.58) 

Thus, (cos 9 1 sin 9) gives the unit normal to the line, and p gives the perpen- 
dicular distance to the line from the origin. Under this parameterization, the 
distance from the line to the point with coordinates (r, c) is given by 

d(r,c) = r cos 9 + csmd — p. (11.59) 


Thus, our task is to find 


C * = inip y^(r cos 9 + csinf? — p) 2 J(r, c) 


(11.60) 
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We compute the partial derivative with respect to p as 


— C = — y^(r cos0 + csin0 — p) 2 X(r, c) 

dp dp ' v ' 

r,c 

(11.61) 

= — 2y^(rcosfl + csinfl — p)l(r,c ) 

r,c 

(11.62) 

= —2 cos 6 ^ rl(r, c) — 2 sin 9 ^ d(r, c) + 2 p ^ X(r, 

VyC v.c v.c 

c)(11.63) 

= —2 (cos OrriiQ + sin 8mm ~ 00 ) 

(11.64) 

= — 2(moo7 cos 0 + moocsin 0 — pmoo) 

(11.65) 

= — 2moo(rcos0 + csin0 — p). 

(11.66) 

Now, setting this to zero we obtain 


f cos 6 + csmO — p = 0. 

(11.67) 


But this is just the equation of a line that passes through the point ( r, c ), 
and therefore we conclude that the inertia is minimized by a line that passes 
through the center of mass. We can use this knowledge to simplify the remaining 
computations. In particular, define the new coordinates (r , , c') as 

r' = r — r, c' = c — c. (11.68) 

The line that minimizes C passes through the point r' = 0, c! =0, and therefore 
its equation can be written as 

r'cos0 + c'sin# = 0. (11.69) 


Before computing the partial derivative of C (expressed in the new coordinate 
system) with respect to 9 , it is useful to perform some simplifications. 


C = ^(r' cos0 + d sin0) 2 Z(r, c) (11.70) 

r,c 

= cos 2 t ?^(r') 2 X(r, c) + 2cos0sin0^(r'c , )I(r, c) + sin 2 9 ^(c^XfTjt) 

r,c r,c r,c 

= C 20 cos 2 9 + 2Cii cos 9 sin 8 + C 02 sin 2 8 (11.72) 


in which the Cij are the central moments given in ( 11.54 1. Note that the central 
moments depend on neither p nor 6. 

The final set of simplifications that we will make all rely on the double angle 
identities: 


cos 2 9 = 

- + - cos 29 

2 2 

(11.73) 

sin 2 8 = 

cos 29 

2 2 

(11.74) 

cos9 sin 8 = 

- sin 28. 

2 

(11.75) 


352 COMPUTER VISION 


Substituting these into our expression for C we obtain 


£ = C 20 (^ + ^cos20) + 2C'n(^sin20) ^ cos 20) (11.76) 

= ~j(C2Q + C$ 2 ) + ~^{C2q — Cm) cos29 + ^Cnsm.26 (11.77) 
It is now easy to compute the partial derivative with respect to 0: 

-jjj£ = — -(C 2 q + Co 2 ) + -(C 2 q — C'o 2 ) cos 20 + -Cn sin 20 (11.78) 


= -(C 20 - C 02 ) sin 20 + Cn cos 20, 

and we setting this to zero we obtain 

C11 


tan 20 = 


C 20 ~ C, 


02 


(11.79) 


(11.80) 


Figure |11.7 shows the orientations for the connected components of the image 
of Figure 11. 3[ 
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Problems 

TO BE WRITTEN 







VISION-BASED 

CONTROL 


In Chapter [9] we described how feedback from a force sensor can be used to 
control the forces and torques applied by the manipulator. In the case of force 
control, the quantities to be controlled (i.e., forces and torques) are measured 
directly by the sensor. Indeed, the output of a typical force sensor comprises 
six electric voltages that are proportional to the forces and torques experienced 
by the sensor. Force control is very similar to state-feedback control in this 
regard. 

In this chapter, we consider the problem of vision-based control. Unlike 
force control, with vision-based control the quantities to be controlled cannot be 
measured directly from the sensor. For example, if the task is to grasp an object, 
the quantities to be controlled are pose variables, while the vision sensor, as we 
have seen in Chapter EH provides a two-dimensional array of intensity values. 
There is, of course, a relationship between this array of intensity values and 
the geometry of the robot’s workspace, but the task of inferring this geometry 
from an image is a difficult one that has been at the heart of computer vision 
research for many years. The problem faced in vision-based control is that of 
extracting a relevant and robust set of parameters from an image and using 
these to control the motion of the manipulator in real time. 

Over the years, a variety of approaches have been taken to the problem 
of vision-based control. These vary based on how the image data is used, 
the relative configuration of camera and manipulator, choices of coordinate 
systems, etc. In this chapter, we focus primarily on one specific approach: 
image-based visual servo control for eye-in-hand camera configurations. We 
begin the chapter with a brief description of this approach, contrasting it with 
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other options. Following this, we develop the specific mathematical tools needed 
for this approach, both design and analysis. 


12.1 APPROACHES TO VISION BASED-CONTROL 

There are several distinct approaches to vision-based control. These vary based 
primarily on system configuration and how image data is used. In this section, 
we give a brief description of these considerations. 


12.1.1 Where to put the camera 


Perhaps the first decision to be made when constructing a vision-based control 
system is where to place the camera. There are essentially two options: the 
camera can be mounted in a fixed location in the workspace, or it can be 
attached to the manipulator. These are often referred to as fixed camera vs. 
eye-in-hand configurations, respectively. 

With a fixed camera configuration, the camera is positioned so that it can 
observe the manipulator and any objects to be manipulated. There are several 
advantages to this approach. Since the camera position is fixed, the field of 
view does not change as the manipulator moves. The geometric relationship 
between the camera and the workspace is fixed, and can be calibrated off line. 
A disadvantage to this approach is that as the manipulator moves through the 
workspace, it can occlude the camera’s field of view. This can be particularly 
important for tasks that require high precision. For example, if an insertion task 
is to be performed, it may be difficult to find a position from which the camera 
can view the entire insertion task without occlusion from the end effector. 

With an eye-in-hand system, the camera is often attached to the manipu- 
lator above the wrist, i.e., the motion of the wrist does not affect the camera 
motion. In this way, the camera can observe the motion of the end effector 
at a fixed resolution and without occlusion as the manipulator moves through 
the workspace. One difficulty that confronts the eye- in-hand configuration is 
that the geometric relationship between the camera and the workspace changes 
as the manipulator moves. The field of view can change drastically for even 
small motion of the manipulator, particularly if the link to which the camera is 
attached experiences a change in orientation. For example, a camera attached 


to link three of an elbow manipulator (such as the one shown in Figure 3.1 1 
will experience a significant change in field of view when joint 3 moves. 

For either configuration, motion of the manipulator will produce changes in 
the images obtained by the camera. The analysis of the relationships between 
manipulator motion and changes for the two cases are similar, and in this text 
we will consider only the case of eye-in-hand systems. 


CAMERA MOTION AND INTERACTION MATRIX 357 


12.1.2 How to use the image data 

There are two basic ways to approach the problem of vision-based control, and 
these are distinguished by the way in the data provided by the vision system 
is used. These two approaches can also be combined in various ways to yield 
what are known as partitioned control schemes [?]. 

The first approach to vision-based control is known as position-based visual 
servo control. With this approach, the vision data is used to build a partial 
3D representation of the world. For example, if the task is to grasp an object, 
the perspective projection equations from Chapter [Tl] can be solved to deter- 
mine the 3D coordinates of the grasp points relative to the camera coordinate 
frame. If these 3D coordinates can be obtained in real time, then they can be 
provided as set points to the robot controller, and control techniques described 
in Chapter [7] can be used. The main difficulties with position-based methods 
are related to the difficulty of building the 3D representation in real time. In 
particular, these methods tend to not be robust with respect to errors in cali- 
bration. Furthermore, with position-based methods, there is no direct control 
over the image itself. Therefore, a common problem with position-based meth- 
ods is that camera motion can cause the object of interest to leave the camera 
field of view. 

A second method known as image-based visual servo control directly uses 
the image data to control the robot motion. An error funtion is defined in 
terms of quantities that can be directly measured in an image (e.g., image 
coordinates of points, the orientation of lines in an image), and a control law is 
constructed that maps this error directly to robot motion. To date, the most 
common approach has been to use easily detected points on an object as feature 
points. The error function is then the vector difference between the desired and 
measured locations of these points in the image. Typically, relatively simple 
control laws are used to map the image error to robot motion. We will describe 
image-based control in some detail in this chapter. 

Finally, these two approaches can be combined by using position-based meth- 
ods to control certain degrees of freedom of the robot motion and using image- 
based methods to control the remaining degrees of freedom. Such methods 
essentially partition the set of degrees of freedom into disjoint sets, and are 
thus known as partitioned methods. We briefly describe a partitioned method 
in Section fl 2. 6 1 


12.2 CAMERA MOTION AND INTERACTION MATRIX 

As mentioned above, image-based methods map an image error function di- 
rectly to robot motion without solving the 3D reconstruction problem. Recall 
the inverse velocity problem discussed in Chapter [4] Even though the inverse 
kinematics problem is difficult to solve and often ill posed (because the solution 
is not unique), the inverse velocity problem is typically fairly easy to solve: 
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one merely inverts the manipulator Jacobian matrix (assuming the Jacobian is 
nonsingular) . This can be understood mathematically by noting that while the 
inverse kinematic equations represent a nonlinear mapping between possibly 
complicated geometric spaces (e.g., even for the simple two-link planar arm the 
mapping is from R 2 to the torus), the mapping of velocities is a linear map 
between linear subspaces (in the two-link example, a mapping from SR 2 to a 
plane that is tangent to the torus). Likewise, the relationship between vectors 
defined in terms of image features and camera velocities is a linear mapping 
between linear subspaces. We will now give a more rigorous explanation of this 
basic idea. 

Let s(t) denote a vector of feature values that can be measured in an image. 
Its derivative, s(t) is referred to as to as an image feature velocity. For 
example, if a single image point is used as a feature, we would have s(t) = 
(u(t),v(t)) T . In this case, s(t) would be the image plane velocity of the image 
point. 

The image feature velocity is linearly related to the camera velocity. Let the 
camera velocity £ consist of linear velocity v and angular velocity u>, 


£ = 



( 12 . 1 ) 


i.e. , the origin of the camera frame is moving with linear velocity v, and the 
camera frame is rotating about the axis to which is rooted at the origin of the 
camera frame as shown in Figure 12.1 The relationship between s and £ is 
given by 


s = i(s,g)£. 


( 12 . 2 ) 


Here, the matrix L(s, q) is known as the interaction matrix or image Jacobian 
matrix. It was first introduced in [Ml , where it was referred to it as the feature 
sensitivity matrix. In [27] it was to as a Jacobian matrix (subsequently referred 
to in the literature as the image Jacobian), and in [24] it was given the name 
interaction matrix, the term that we will use. Note that the interaaction matrix 
is a function of both the configuration of the robot (as was also true for the 
manipulator Jacobian discribed in Chatper[4]) and of the image feature values, 
s. 

The specific form of the interaction matrix depends on the features that are 
used to define s. The simplest features are coordinates of points in the image, 
and we will focus our attention on this case. 


12.2.1 Interaction matrix vs. Image Jacobian 

The interaction matrix L is often referred to in the visual servo community 
by the name image Jacobian matrix. This is due, at least in part, to the 
analogy that can be drawn between the manipulator Jacobian discussed in 
Chapter [4] and the interaction matrix. In each case, a velocity £ is related 
to the variation in a set of parameters (either joint angles or image feature 
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Figure showing the linear and angular 
velocity vectors for the camera frame, 
to be created 


Fig. 12.1 Camera velocity 


velocities) by a linear transformation. Strictly speaking, the interaction matrix 
is not a Jacobian matrix, since £ is not actually the derivative of some set of 
pose parameters. However, using techniques analogous to those used to develop 
the analytic Jacobian in Section [4. 8[ it is straightforward to construct an actual 
Jacobian matrix that represents a linear transformation from the derivatives of 
a set of pose parameters to the image feature velocities (which are derivatives 
of the image feature values). 


12.3 THE INTERACTION MATRIX FOR POINTS 


In this section we derive the interaction matrix for the case of a moving camera 
observing a point that is fixed in space. This scenario is useful for postioning a 
camera relative to some object that is to be manipulated. For exaple, a camera 
can be attached to a manipulator arm that is to grasp a stationary object. 
Vision-based control can then be used to bring the manipulator to a grasping 


configuration that may be defined in terms of image features. In section 12.3.4 
we extend the development to the case of multiple feature points. 

At time t, the orientation of the camera frame is given by a rotation matrix 
R° c = R(t), which specifies the orientation of the camera frame at time t rel- 
ative to the fixed frame. We denote by 0(t) the position of the origin of the 
camera frame relative to the fixed frame. We denote by p the fixed point in the 
workspace, and s = (it, v) T is the feature vector corresponding to the projection 
of this point in the image. 

Our goal in this section is to derive the interaction matrix L that relates the 
velocity of the camera £ to the derivatives of the coordinates of the projection 
of the point in the image s. We begin by finding an expression for the relative 
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velocity of the point p to the moving camera. We then use the perspective 
projection equations to relate this velocity to the image velocity s. Finally, 
after a bit of algebraic manipulations we arrive to the interaction matrix that 
satisfies s = LI;. 


12.3.1 Velocity of a fixed point relative to a moving camera 


We denote by p° the coordinates of p relative to the world frame. Note that 
p° does not vary with time, since p is fixed with respect to the world frame. If 
we denote by p{t) the coordinates of p relative to the moving camera frame at 
time t , we have 

p° = R(t)p(t) + 0(t). (12.3) 


Thus, at time t we can solve for the coordinates of p relative to the camera 
frame by 

p(t) = R T (t)p° -R T (t)0(t), (12.4) 


which is merely the time varying version of Equation (2.55). Now, to find 
the velocity of the point p relative to the moving camera frame, we merely 
differentiate this equation (as in Chapter |4|. We will drop the explicit reference 
to time in these equations to simplify notation, but the reader is advised to 
bear in mind that both the rotation matrix R and the location of the origin of 
the camera frame O are time varying quantities. The derivative is computed as 
follows 



i [*V] - | [R t o] 


dt 


T R 

dt 

T R 

dt 


P°~ 


±R 

dt 


O-R 1 —O 
dt 


(p° - o) - r t o 


(12.5) 


In this equation, the quantity p° — O is merely the vector from the origin 
of the moving frame to the fixed point p , expressed in coordinates relative to 
the fixed frame, and thus R T (p° — O) = p is the vector from the origin of the 
moving fr ame t o the point p expressed relative to the moving frame. Using 
Equation (4.191, we can write the derivative of R as R = S(lo)R , which allows 
us to write Equation (112. 5| as 


= [5(w)1?] T (p° -O)- R t O 
= R t S t (u>) (p° - O) - R t O 
= R t [S(-w) (p° - O)] - R t O 
= - R t lo x R t (p° - O) - R t O 


The vector u> gives the angular velocity vector for the moving frame expressed 
relative to the fixed frame, i.e., u = u>°. Therefore, R t oj = RqU>° = oj c gives 
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the angular velocity vector for the moving frame relative to the moving frame. 
Similarly, note that R T O = O c . Using these conventions, we can immediately 
write the equation for the velocity of p relative to the moving camera frame 

p=-u c xp-O c (12.6) 

It is interesting to note that this velocity of a fixed point relative to a moving 
frame is merely —1 times the velocity of a moving point (i.e., a point attached 
to a moving frame) relative to a fixed frame. 

Example 12.1 Camera motion in the plane 

TO BE WRITTEN: camera motion is in the plane parallel to the image 
plane (i.e., rotation about optic axis, translation parallel to camera x-y axes). 
Compute the velocity of a fixed point relative to the camera frame 
o 


12.3.2 Constructing the Interaction Matrix 


To simplify notation, we define the coordinates for p relative to the camera frame 
as p = ( x,y,z) T . By this convention, the velocity of p relative to the moving 
frame is merely the vector p = (x,y,z) T . We will denote the coordinates 
for the angular velocity vector by lo c = {u> x ,LO y ,u) z ) T = R t uj. To further 
simplify notation, we assign coordinates R T O = {v x ,v y ,v z ) T = O c . Using 
these conventions, we can write Equation (12.61 as 


X 


U X 


X 


V x 

y 

= — 

Uy 

X 

y 

- 

Vy 

z 


LO z 


z 


. W 


which can be written as the system of three equations 


x = yuj z - ZLO y - v x 

y = zco x - xlo z - v y 

z = xujy - yu) x - v z 


(12.7) 

(12.8) 
(12.9) 


Assuming that the imaging geometry can be modeled by perspective pro- 
jection as given by Equation (11.5), we can express x and y in terms of image 
coordinates u, v of the projection of p in the image and the depth 2 as 


uz 



vz 



Substituting these into Equations ( 12.7 >-( 12.9 > we obtain 


x 


y 

z 


vz 

—UJ Z - ZLOy - V x 
UZ 

ZLO x - -J-Uz + -Vy 


uz 

T - y 


VZ 

rWx - V z 


(12.10) 

( 12 . 11 ) 

(12.12) 
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These equations express the velocity p in terms of the image coordinates u, v 
the depth of the point p, and the angular and linear velocity of the camera. 
Our goal is to derive an expression that relates the image velocity (ft, v) T to the 
angular and linear velocity of the camera frame. Therefore, w e will n ow find 
expressions for (ft, v) T and then combine these with Equations ( 12.10 (-(12.12 ). 

Using the quotient rule for differentiation with the equations of perspective 
projection we obtain 


( 12 . 10 )-( 12.12 


d Xx zx — xz 
“ = 57 = 1 — 


Substituting Equations (12.10) and (12.12) into this expression gives 


u 


#\ z 


vz 

u 


■LO z - ZU1 V - V x 


UZ 

T 


UZ vz 
—\ wv ~\ u)x 



) 


A u uv A 2 + u 2 

V x + ~V Z + T W X r UJ y + VLO z 

Z Z A A 


(12.13) 


We can apply the same technique for v 


v = 


d_ Ay _ ^ zy - yz 

dt z z 2 


and substituting Equations (12.11) and (12.121 into this expression gives 



A 

( 

UZ 

VZ 

UZ 

vz 

\ 

V = 


(* 

- Vy 

T 

Vx^y- 

— u x - v z 

) 


A v A 2 + v 2 uv 

= Vy + -V Z H U X —U)y - UUJ Z 

Z Z A A 


(12.14) 


Equations (12.13) and (12.14) can be combined and written in matrix form 


as 


u 

v 


A 

z 

0 


u uv 

z X 

A v A 2 + v 2 

z z A 


A 2 + u 2 
A 

uv 


V 

—u 


Vx 

Vy 

V z 

‘-Vx 

LOy 

Ul z 


(12.15) 


The matrix in this equation is the interaction matrix for a point. It relates 
the velocity of the camera to the velocity of the image prjection of the point. 
Note that this interaction matrix is a function of the image coordinates of the 
point, u and v, and of the depth of the point with respect to the camera frame, 
z. Therefore, this equation is typically written as 


s = L p (u,v,z)£ 


(12.16) 


Example 12.2 Camera motion in the plane (cont.) 

TO BE WRITTEN: Give specific image coordinates to continue the previous 
example and compute the image motion as a function of camera velocity. 


o 
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12.3.3 Properties of the Interaction Matrix for Points 


Equation (12.16) can be decomposed and written as 


s = L v (u, v, z)v + L u (u, v)u 


(12.17) 


in which L v (u,v,z) contains the first three columns of the interaction matrix, 
and is a function of both the image coordinates of the point and its depth, 
while L u (u , v) contains the last three columns of the interaction matrix, and is 
a function of only the image coordinates of the point (i.e. , it does not depend 
on depth). This can be particularly beneficial in real-world situations when 
the exact value of z may not be known. In this case, errors in the value of z 
merely cause a scaling of the matrix L v (u,v, z), and this kind of scaling effect 
can be compensated for by using fairly simple control methods. This kind of 
decomposition is at the heart of the partitioned methods that we discuss in 
Section 112.61 

The camera velocity £ has six degrees-of- freedom, while only two values, u 
and v, are observed in the image. Thus, one would expect that not all camera 
motions case observable changes in the image. More precisely, L £ R 2x6 and 
therefore has a null space of dimension 4, i.e., the system 


0 = L(s,g)£ 


has soltion vectors £ that lie in a four-dimensional subspace of R 6 . For the case 
of a single point, it can be shown that the null space of the interaction matrix 
given in (12.151 is spanned by the four vectors 


u 


' 0 ' 


uvz 


X (u 2 + v 2 + A 2 )z 

V 


0 


-(u 2 + X 2 )z 


0 

X 


0 


A vz 


—u(u 2 + v 2 + X 2 )z 

0 


u 


-A 2 


uvX 

0 


V 


0 


-( u 2 + A 2 )z 

0 


_ A _ 


uX 


uX 2 


The first two of these vectors have particularly intuitive interpretations. The 
first corresponds to motion of the camera frame along the projection ray that 
contains the point p, and the second corresponds to rotation of the camera 
frame about a projection ray that contains p. 


12.3.4 The Interaction Matrix for Multiple Points 

It is straightforward to generalize the development above the case in which 
several points are used to define the image feature vector. Consider the case for 
which the feature vector vector consists of the coordinates of n image points. 
Here, the i th feature point has an associated depth, Zi , and we define the feature 
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vector s and the vector of depth values, z by 


Ml 

Vl 


s = 


and z = 


z 1 


U n 

V„ 


z 


n 


For this case, the composite interaction matrix L c that relates camera ve- 
locity to image feature velocity is a function of the image coordinates of the n 
points, and also of the n depth values, 


s = L c (s,z)£ 


This interaction matrix is thus obtained by stacking the n interaction matrices 
for the individual feature points, 


Li{ui,vi,zi) 


L n {LL n i V n , Zn) 


X 

0 

Ml 

UiVi 

A 2 + u\ 

■ci 

Zl 

Zl 

A 

A 

0 

A 

Vi 

X 2 + v\ 

UiV i 

-Mi 

Zl 

Zl 

A 

A 


A 

0 

u n 

'U'n'Vn 

A" + M 2 

V n 

Zn 

Zn 

A 

A 

0 

A 

V n 

A" + m 2 

^n^n 

-u„ 

Zn 

Zn 

A 

A 


Thus, we have L c £ R 2nx6 anc l therefore three points are sufficient to solve 
for £ given the image measurements s. 


12.4 IMAGE-BASED CONTROL LAWS 

With image-based control, the goal configuration is defined by a desired con- 
figuration of image features, denoted by s d . The image error function is then 
given by 


e(t) = s(t) — s d . 
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The image-based control problem is to find a mapping from this error function 
to a commanded camera motion, u(t). As we have seen in previous chapters, 
there are a number of control approaches that can be used to determine the 
joint-level inputs to achieve a desired trajectory. Therefore, in this chapter 
we will treat the manipulator as a kinematic positioning device, i.e., we will 
ignore manipulator dynamics and develop controllers that compute desired end 
effector trajectories. The underlying assumption is that these trajectories can 
then be tracked by a lower level manipulator controller. 

The most common approach to image-based control is to compute a desired 
camera velocity and use this as the control u(t) = £. Relating image feature 
velocities to the camera velocity £ is typically done by solving Equation (12.21. 
Solving this equation will give a desired camera velocity. In some cases, this 
can be done simply by inverting the interaction matrix, but in other cases the 
pseudoinverse must be used, as described below. 


12.4.1 Computing Camera Motion 

For the case of k feature values and m components of the camera body velocity 
£, we have L £ 3? fcxm . In general we will have m = 6, but in some cases 
we may have m < 6, for example if the camera is attached to a SCARA arm 
used to manipulate objects on a moving conveyor. When L is full rank (i.e., 
rank(L) = min (fc,?n)), it can be used to compute £ from s. There are three 
cases that must be considered: k = m, k > m, and k < m. We now discuss 
each of these. 

When k = m and L is full rank, L is nonsingular, and L _1 exists. Therefore, 
in this case, £ = A -1 s. 

When k < to, L _1 does not exist, and the system is underconstrained. In 
the visual servo application, this implies that the we are not observing enough 
feature velocities to uniquely determine the camera motion £, i.e., there are 
certain components of the camera motion that can not be observed. In this 
case we can compute a solution given by 


£ = L+s+ (I- L+L)b 

where L + is the pseudoinverse for L given by 

L+ = L t (LL t )~ 1 

and b £ is an arbitrary vector. Note the similarity between this equation 


and Equation (4.1281 which gives the solution for the inverse velocity problem 


(i.e., solving for joint velocities to achieve a desired end-effector velocity) for 
redundant manipulators. 

In general, for k < m, (I — LL + ) ^ 0, and all vectors of the form (/ — LL + )b 
lie in the null space of L , which implies that those components of the camera 
velocity that are unobservable lie in the null space of L. If we let b = 0, we 
obtain the value for £ that minimizes the norm 
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P-^II 

When k > m and L is full rank, we will typically have an inconsistent system 
(especially when s is obtained from measured image data). In the visual servo 
application, this implies that the we are observing more feature velocities than 
are required to uniquely determine the camera motion £. In this case the rank 
of the null space of L is zero, since the dimension of the column space of L 
equals rank(L). In this situation, we can use the least squares solution 

£ = L+s (12.18) 

in which the pseudoinverse is given by 

L + = (L t L)" 1 L t (12.19) 


12.4.2 Proportional Control Schemes 

Many modern robots are equipped with controllers that accept as input a com- 
mand velocity £ for the end effector. Thus, we define our control input as 
u(t) = £. Using the results above, we can define a proportional control law as 

u{t) = - KL+e(t ). (12.20) 

in which K is an m x m diagonal, positive definine gain matrix. 

The derivative of the error function is given by 

e(t) = 4( S W “ = «(*) = 

at 


and substituting Equation (12.201 for £ we obtain 

e(t) = —KLL + e(t) 

If k = m and L has full rank, then L + = L _1 , and we have 

e(t) = —Ke(t) 


(12.21) 


From linear system theory, we know that this system is stable when the eigen 
values of K are positive, which motivates the selection of K as a positive definite 
matrix. 

When k > m, as is typically the case for visual servo systems, a sufficient 
condition for stability is that the matrix product KLL + is positive definite. 
This is easily demonstrated using the Lyapunov function 

V(t) = ^||e(f)|| 2 = ^e T e 
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Using Equation (12.21 1 the derivative V is given by 

• d 1 r T 1 

V = —~e T e 


dt 2 

e 

= —e T ( KLL + ) 


T • 

= e e 


and we see that V < 0 when KLL + is positive definite. 

In practice, we will not know the exact value of L or L + since these depend 
on knowledge of depth information that must be estimated by the computer 
vision system. In this case, we will have an estimate for the interaction matrix 
L + and we can use the control u(t) = — KL + e{t ). It is easy to show, by a 
proof analogous to the one above, that the resutling visual servo system will 
be stable when KLL + is positive definite. This helps to explain the robustness 
of image-based control methods to calibration errors in the computer vision 
system. 


12.5 THE RELATIONSHIP BETWEEN END EFFECTOR AND 
CAMERA MOTIONS 


The output of a visual servo controller is a camera velocity £ c , typically expresed 
in coordinates relative to the camera frame. If the camera frame were coincident 
with the end effector frame, we could use the manipulator Jacobian to determine 
the joint velocities that would achieve the desired camera motion as described 
in Section |4.10| In most applications, the camera frame is not coincident with 
the end effector frame, but is rigidly attached to it. In this case, the two frames 
are related by the constant homogeneous transformation 


rpK) 


R 6 c 4 

o 1 


(12.22) 


Our goal is to find the relationship betweent the body velocity of the cam- 
era fame £ c = (v c ,oj c ) t and the body velocity of the end effector frame ^6 = 
( i>Q,uJe ) T . Furthermore, we will assume that the camera velocity is given with 
respect to the camera frame (i.e., we are given the coordinates £(;) , and that 
we wish to determine the end effector velocity relative to the end effector frame 
(i.e., we wish to find the coordinates £g). Once we obtain it is a simple 
matter to express £ = (vq ,u>q) t with respect to the base frame, as we will see 
below. 

Since the two frames are rigidly attached, the angular velocity of the end 
effector frame is the same as the angular velocity for the camera frame. An 
easy way to show this is by computing the angular velocities of each frame by 
taking the derivatives of the appropriate rotation matrices (such as was done 
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in Chapter [4]). The derivation is as follows, 


R° c 

— ?? 0 
dt R,: 

K 

S(co° c )R° c 

S(u° c ) 


TD 0 06 

U 6 n c 

d nO p6 
dt R 6 R c 

zS 0 TD 6 

U 6 n c 

S(lo° 6 )R° 6 R 6 c 

5K°) 


Thus, we have = Wg, and it is clear that the angular velocity of the end 
effector is identical to the angular velocity of the camera frame, u>§ = ui c . If the 
coordinates of this angular velocity are given with respect to the camera frame 
and we wish to express the angular velocity with respect to the end effector 
frame, we merely use the rotational coordinate transformation 


< = R b Mc (12-23) 

If the camera is moving with body velocity £ = {v c ,oj c ) t , then the linear 
velocity of the origin of the end effector frame (which is rigidly attached to the 
camera frame) is given by v c + u> c x r, with r the vector from the origin of the 
camera frame to the origin of the end effector frame. From Equation (12.221, 
d® gives the coordinates of the origin of the camera frame with respect to the 
end effector frame, and therefore we can express r in coordinates relative to the 
camera frame as r c = — i?gd®. Thus, we write u> c x r in the coordinates with 
respect to the camera frame as 


Wc c x {-Rldt) = R c 6 dt x Wc c 

Now to express this free vector with respect to the end effector frame, we 
merely apply a rotation transformation, 

R 6 c (R c 6 d 6 c xu‘) = dtxRlu c c 

= S(d 6 c )R 6 c u c c (12.24) 


Expressing v c relative to the end effector frame is also accomplished by a 
simple rotational transformation, 


= R>: 


6 „,c 


(12.25) 


Combining Equations (12.231, ( 12.24[ ), and ( |12.25 1 into a single matrix equa- 
tion, we obtain 


£ 


6 

6 


R6 c S(d e c )R 6 c 

03x3 R 6 C 


S 


c 

c 


If we wish to express the end effector velocity with respect to the base frame, 
we merely apply a rotational transformation to the two free vectors vq and uiq, 
and this can be written as the matrix equation 


a 


0 

6 


iig o 3x3 

03X3 R° 6 


Z 


6 
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Example 12.3 Eye-in-hand system with SCARA arm 

TO BE WRITTEN: This example will show the required motion of the SCARA 
arm to cause a pure rotation about the optic axis of the camera , for the camera 
attached to the end effector, optic axis of the camera parallel to world z-axix. 
o 


12.6 PARTITIONED APPROACHES 


TO BE REVISED 

Although imgae-based methods are versatile and robust to calibration and 
sensing errors, they sometimes fail when the required camera motion is large. 
Consider, for example, the case when the required camera motion is a large 
rotation about the optic axis. If point features are used, a pure rotation of 
the camera about the optic axis would cause each feature point to trace a 
trajectory in the image that lies on a circle. Image-based methods, in contrast, 
would cause each feature point to move in a straight line from its current image 
position to its desired position. The induced camera motion would be a retreat 
along the optic axis, and for a required rotation of it, the camera would retreat 
to z = — oo, at which point detL = 0, and the controller would fail. This 
problem is a consequence of the fact that image-based control does not explicitly 
take camera motion into acount. Instead, image-based control determines a 
desired trajector in the image feature space, and maps this trajectory, using 
the interaction matrix, to a camera velocity. 

To combat this problem, a number of partitioned methods have been 
introduced. These methods use the interaction matrix to control only a subset 
of the camera degrees of freedom, using other methods to control the remaining 


degrees of freedom. Consider Equation (12.151. We can write this equation as 


A n uv A 2 + u 2 

T A 

A A 2 + v 2 uv 

" A 



v x 


u 




— V 


V v 
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y 

+ 



W X 


V 




- —u 


. u y . 
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V z 

L0 Z 


If we genaralize this to k feature points, we stack the resulting interaction 


matrices as in Section 12.3.4 and the resulting relationship is given by 

^ — Lxyfxy T B z ^ z 


= s 


xy 


(12.26) 

(12.27) 


In Equation (12.271, s z = L z ^ z gives the component of s due to the camera 
motion along and rotation about the optic axis, while s xy = L xy f xy gives the 
component of s due to velocity along and rotation about the camera x and y 


axes. 
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Fig. 12.2 Feature used to determine w z . 


* xy 


Equation (12.261 allows us to partition the control u into two components, 
= £,xy and u z = £ 2 . Suppose that we have established a control scheme to 


determine the value £ 2 = u z . Using an image-based method to find u xy , we 


would solve Equation (12.261 for 


^xyi 




ty {s L z £ z } 

= L xy {® ~ ®z} 


(12.28) 

(12.29) 


This equation has an intuitive explanation. — L+ y L 2 £ 2 
to cancel the feature motion s,. The control u r 


Sxy 


is the required value of 
xy = €xy = L+ y s gives the 
velocity along and rotation about the camera x and y axes that produce the 
desired s once image feature motion due to £ 2 has been accounted for. 

In }15j . £ 2 , is computed using two image features that are simple and com- 
putationally inexpensive to compute. The image feature used to determine lu z 
is dij, with 0 < Oij < 2-7T the angle between the horizontal axis of the image 
plane and the directed line segment joining feature points two feature point. 
This is illustrated in Figure 12.2 For numerical conditioning it is advantageous 


to select the longest line segment that can be constructed from the feature 
points, and allowing that this may change during the motion as the feature 
point configuration changes. The value for u> z is given by 




in which 0f ;] is the desired value, and r ) ulz is a scalar gain coefficient. This form 
allows explicit control over the direction of rotation, which may be important 
to avoid mechanical motion limits. For example if a hard stop exists at 6 S then 


w 2 = 7w*sgn (Ofj - (9 s )sgn(% - 0 S ) [d^ - %] 


will avoid motion through that stop. 

The image feature used to determine v z is the square root of the area of the 
regular polygon enclosed by the feature points. If we denote the area of the 
polygon by a 2 , we determine v z as 


v z 


cr d 

lv z ln(— ) 

<7 


(12.30) 
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Fig. 12.3 Feature used to determine v z . 


TO APPEAR 


Fig. 12.4 Proposed partitioned IBVS for pure target rotation (7rrad). (a) Image-plane 
feature motion (initial location is o, desired location is •), (b) Feature error trajectory, 
(c) Cartesian translation trajectory. 


The advantages of this approach are that (1) it is a scalar; (2) it is rotation 
invariant thus decoupling camera rotation from Z-axis translation. (3) it can 
be cheaply computed. 

Figure 12.4 shows the performance of the proposed partitioned controller for 
the case of desired rotation by ir about the optic axis. The important features 
are that the camera does not retreat since a is constant at a = 0. The rotation 
9 monotonically decreases and the feature points move in a circle. The feature 
coordinate error is initially increasing, unlike the classical IBVS case in which 
feature error is monotonically decreasing. 

An example that involves more complex translational and rotational motion 
is shown in Figure |I 2. 5 The new features decrease monotonically, but the error 
in s does not decrease monotonically and the points follow complex curves on 
the image plane. Figure [i2.6| compares the Cartesian camera motion for the two 
IBVS methods. The proposed partitioned method has eliminated the camera 
retreat and also exhibits better behavior for the X- and Y-axis motion. However 
the consequence is much more complex image plane feature motion that admits 
the possibility of the points leaving the field of view. 

Other partitioned methods have been propose in mmmi but these rely 
on advanced concepts from projective geometry, and are beyond the scope of 
this text. 
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Fig. 12.5 Proposed partitioned IBVS for general target motion, (a) Image-plane feature 
motion (dashed line shows straight line motion for classical IBVS), (b) Feature error 
trajectory. 
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Fig. 12.6 Comparison of Cartesian camera motion for classic and new partitioned IBVS 
for general target motion. 


12.7 MOTION PERCEPTIBILITY 


Recall the that notion of manipulability described in Section |4.11| gave a quan- 
titative measure of the scaling from joint velocities to end-effector velocities. 
Motion pereptibility |69j, 68 is an analogous concept that relates camera 
velocity to the velocity of features in the image. The notion of resolvability 
introduced in [551 l56j is similar. Intuitively, motion perceptibility quantifies the 
magnitude of changes to image features that result from motion of the camera. 

Consider the set of all robot tool velocities £ such that 

ll£ll = (£i 2 +£ 2 2 + ---d) 1/2 <l- (12.31) 


As above, there are three cases to consider. Suppose that k > m (i.e., there are 
redundant image features). We may use Equation (12.181 to obtain 


uw = e -a 

= ( L+s) T (L+s ) 

= s t (L +T L + )s < 1 (12.32) 

Now, consider the singular value decomposition of L , given by 


L = UT,V t . 


(12.33) 
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in which 


U = [U!U 2 ■ . . U k ] , V = [V\V2 


are orthogonal matrices, and E £ ?ft kxm with 


(12.34) 


E = 


fi 


C2 


0 


(12.35) 


and the a i are the singular values of L, and <j\ > a 2 ■ ■ ■ > a m . 

For this case, the pseudoinverse of the image Jacobian, L + , is given by 


Equation (12.19). Using this with Equations (12.321 and (12.33) we obtain 


s 2 U 


-2 




U 1 s < 1 


(12.36) 


Consider the orthogonal transformation of s given by 


~s=U T s 


Substituting this into Equation (12.361 we obtain 


rr 2 Si - 1 


i= 1 


(12.37) 


(12.38) 


Equation (12.381 defines an ellipsoid in an m-dimensional space. We shall refer 
to this ellipsoid as the motion perceptibility ellipsoid. We may use the volume 
of the m-dimensional ellipsoid given in ( 12.38 1 as a quantitative measure of the 
perceptibility of motion. The volume of the motion perceptibility ellipsoid is 
given by 

K^det(L T L), (12.39) 


in which K is a scaling constant that depends on the dimension of the ellipsoid, 
m. Because the constant K depends only on m, it is not relevant for the purpose 
of evaluating motion perceptibility (since m will be fixed for any particular 
problem). Therefore, we define the motion perceptibility, which we shall denote 
be w v , as 


w v 


\J det(L T L) 


— o\o 2 ■ ■ ■ a m . 


(12.40) 
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The motion perceptibility measure, w v , has the following properties, which 
are direct analogs of properties derived by Yoshikawa for manipulability [78] . 

• In general, w v = 0 holds if and only if rank(L) < min (i.e., when 
L is not full rank). 

• Suppose that there is some error in the measured visual feature velocity, 
As. We can bound the corresponding error in the computed camera 
velocity, A£, by 

s pf s < 12 - 41 > 

There are other quantitative methods that could be used to evaluate the per- 
ceptibility of motion. For example, in the context of feature selection, Feddema 
|26j has used the condition number for the image Jacobian, given by ||L|| ||L -1 ||. 


12.8 CHAPTER SUMMARY 
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Problems 
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Appendix A 

Geometry and 
Trigonometry 


A.l TRIGONOMETRY 
A. 1.1 Atan2 

The function 9 = Atan(x, y) computes the arc tangent function, where x and 
y are the cosine and sine, respectively, of the angle 9. This function uses the 
signs of x and y to select the appropriate quadrant for the angle 9. Specifically, 
Atan(a;,y) is defined for all (x,y) ^ (0,0) and equals the unique angle 9 such 
that 


cos 9 = 



sin 9 = 


V 

(. x 2 + y 2 ) 


1 • 

2 


(A.l) 


For example, Atan(l, — 1) = — j, while Atan(— 1,1) = Note that if both 
x and y are zero, A tan is undefined. 
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A. 1.2 Reduction formulas 

sin(— 9) = — sin 6 

cos( — 9) = cos 9 
tan(— 9) = — tan 9 

A. 1.3 Double angle identitites 


sin(| + 8) = cos 6 
tan(f + 8) = — cot 8 
tan(0 — 7r) = tan 8 


sin (a’ ± y) 
cos (a’ ± y) 

tan(a’ ± y) 


sin x cos y ± cos x sin y 
cos x cos y =F sin x sin y 
tan x ± y 
1 tan x tan y 


A. 1.4 Law of cosines 

If a triangle has sides of length a, b and c, and 8 is the angle opposite the side 
of length a, then 

a 2 = b 2 + <? — 2bccosd (A. 2) 



Appendix B 
Linear Algebra 


In this book we assume that the reader has some familiarity with basic proper- 
ties of vectors and matrices, such as matrix addition, subtraction, multiplica- 
tion, matrix transpose, and determinants. These concepts will not be defined 
here. For additional background see [3]. 

The symbol R will denote the set of real numbers, and R™ will denote the 
usual vector space on n-tuples over R. We use lower case letters a, b, c, x , y, etc., 
to denote scalars in R and vectors in R n . Uppercase letters A, B, C, R, etc., 
denote matrices. Unless otherwise stated, vectors will be defined as column 
vectors. Thus, the statement x £ R” means that 


x 


X\ 


x n 


,lj£R. 


(B.l) 


The vector x is thus an n-tuple, arranged in a column with components xi, . . . , x n . 
We will frequently denote this as 

x= [x 1 ,...,x n ] T (B.2) 


where the superscript T denotes transpose. The length or norm of a vector 
x £ R™ is 


11*11 = (x 2 1 + --- + xl)i. 


(B.3) 
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The scalar product, denoted (x,y), or x T y of two vectors x and y belonging 
to R” is a real number defined by 

< x,y ) = x T y = x 1 y 1 -\ V x n y n . (B.4) 

Thus, 

11*11 = (B.5) 

The scalar product of vectors is commutative, that is, 

(x,y) = (y,x). (B.6) 

We also have the useful inequalities, 

| (ar, 2 /) | < ||x|| H 2 /II (Cauchy-Schwartz) (B.7) 

II* + y\\ < ||*|| + \\y\\ (Triangle Inequality) (B.8) 

For vectors in R 3 the scalar product can be expressed as 

\(x,y)\ = ||*|| ||j/||cos(0) (B.9) 

where 9 is the angle between the vectors x and y. 

The outer product of two vectors x and y belonging to R n is an n x n 
matrix defined by 


T 

xy 


xiyi ■ ■ xiy„ 

x 2 yi ■ • x 2 y n 

XnVl ' ' x n y n 


(B.10) 


From (B.lOl we can see that the scalar product and the outer product are 
related by 


(x,y) = x T y = Tr(xy T ) 


(B.ll) 


where the function TV(-) denotes the trace of a matrix, that is, the sum of the 
diagonal elements of the matrix. 

We will use i, j and k to denote the standard unit vectors in R 3 



■ 1 ' 


' 0 ' 


' 0 ' 

i = 

0 

> 3 = 

1 

, k = 

0 


0 


0 


1 


(B.12) 


Using this notation a vector x = [xi, x 2 , * 3 ] r may be written as 


x 


xi i + x 2 j + x 3 k. 


(B.13) 
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Fig. B.l The right hand rule. 


The vector product or cross product x x y of two vectors x and y be- 
longing to R 3 is a vector c defined by 


c = x x y 


det 


i j k 
xi x 2 x 3 

Vi V2 V3 


(B.14) 


( x 2 y 3 - x 3 y 2 )i + (x 3 yi - x x y 3 )j + (x 3 y 2 - a,’ 2 2/i)fc.(B.15) 


The cross product is a vector whose magnitude is 


INI = INI ||2/|| sin(6>) 


(B.16) 


where 9 is the angle between x and y and whose direction is given by the right 
hand rule shown in Figure |B.1| 

A right-handed coordinate frame x — y — z is a coordinate frame with axes 
mutually perpendicular and that also satisfies the right hand rule as shown 
in Figure |B.2[ We can remember the right hand rule as being the direction of 
advancement of a right-handed screw rotated from the positive x axis is rotated 
into the positive y axis through the smallest angle between the axes. The cross 
product has the properties 


x x y 
x x (y + z) 
a{x x y) 


—y x x 

x x y + x x z 

(ax) x y = x x (ay) 


(B.17) 

(B.18) 


B.l DIFFERENTIATION OF VECTORS 

Suppose that the vector x(t) = (xi(t), . . . ,x n (t)) T is a function of time. Then 
the time derivative x of x Is just the vector 

= (±! (t),...,x n (t)) T 


X 


(B.19) 
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Fig. B.2 The right-handed coordinate frame. 


that is, the vector can be differentiated coordinate wise. Likewise, the derivative 
dA/dt of a matrix A = ( a,j ) is just the matrix (a,j). Similar statements hold 
for integration of vectors and matrices. The scalar and vector products satisfy 
the following product rules for differentiation similar to the product rule for 
differentiation of ordinary functions. 

d , . , dx . , dy. 

dt {x ’ v) = l 'Tf v) + {x 'Lit ) (a20) 

i(ix s ) = (B.21) 

dV dt y dt y ’ 

B.2 LINEAR INDEPENDENCE 

A set of vectors {x\, . . . , x n } is said to linearly independent if and only if 

n 

a.iXi = 0 (B.22) 

i—l 

implies 

Xj = 0 for all i. (B.23) 

The rank of a matrix A is the the largest number of linearly independent rows 

(or columns) of A. Thus the rank of an n x m matrix can be no greater than 
the minimum of n and m. 
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B.3 CHANGE OF COORDINATES 

A matrix can be thought of as representing a linear transformation from R™ to 
K n in the sense that A takes a vector i to a new vector y according to 

y = Ax (B.24) 

y is called the image of x under the transformation A. If the vectors x and y are 
represented in terms of the standard unit vectors i, j, and k, then the columns 
of A are themselves vectors which represent the images of the basis vectors i, j, 
k. Often it is desired to represent vectors with respect to a second coordinate 
frame with basis vectors e, /, and g. In this case the matrix representing the 
same linear transformation as A, but relative to this new basis, is given by 

A' = T~ l AT (B.25) 

where T is a non-singular matrix with column vectors e, /, g. The transforma- 
tion T~ X AT is called a similarity transformation of the matrix A . 


B.4 EIGENVALUES AND EIGENVECTORS 

The eigenvalues of a matrix A are the solutions in s of the equation 

det (si -A) = O. (B.26) 

The function, det(s/ — A) is a polynomial in s called the characteristic poly- 
nomial of A. If s e is an eigenvalue of A, an eigenvector of A corresponding to 
s e is a nonzero vector x satisfying the system of linear equations 

(s e I -A) = 0. (B.27) 


or, equivalently, 


Ax = s e x. (B.28) 

If the eigenvalues Si,...,s n of A are distinct, then there exists a similar- 
ity transformation A' = T _1 AT, such that A' is a diagonal matrix with the 
eigenvalues s\,. . . ,s n on the main diagonal, that is, 

A! = diag[si,...,s„]. (B.29) 

B.5 SINGULAR VALUE DECOMPOSITION (SVD) 

For a square matrices, we can use tools such as the determinant, eigenvalues and 
eigenvectors to analyze their properties. However, for nonsquare matrices these 
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tools simply do not apply. Their generalizations are captured by the Singular 
Value Decomposition (SVD) of a matrix, which we now introduce. 

As we described above, for J £ R mxra , we have JJ T £ R mxm . This square 
matrix has eigenvalues and eigenvectors that satisfy 

JJ T Ui = XiUi (B.30) 

in which A; and Ui are corresponding eigenvalue and eigenvector pairs for JJ T . 
We can rewrite this equation to obtain 

J J T Ui — \ui = 0 

(. JJ T - A il)ui = 0. (B.31) 

The latter equation implies that the matrix ( J J T — Xil) is singular, and we can 
express this in terms of its determinant as 


det(JJ T — Xil) = 0. 


(B.32) 


We can use Equation ( B.32| ) to find the eigenvalues Ai > A 2 • • • > X m > 0 for 
J J T . The singular values for the Jacobian matrix J are given by the square 
roots of the eigenvalues of JJ T , 

fjj = \J Xj . (B.33) 

The singular value decomposition of the matrix J is then given by 


J = U EV t , (B.34) 

in which 


U = [uiu 2 ■ ■ ■ u m \ , V = [v!V 2 ■ • • v n ] (B.35) 

are orthogonal matrices, and E £ R mxn . 


01 


02 



0 

0m 



(B.36) 


We can compute the SVD of J as follows. We begin by finding the singular 
values, cq, of J using Equations (B.32 1 and (B.33 1 . These singular values can 
then be used to find the eigenvectors u \ , • • • u m that satisfy 


JJ T Uj = a^Uj. 


(B.37) 


These eigenvectors comprise the matrix U = [u\ u 2 ■ ■ ■' 
tions (B.37 1 can be written as 


The system of equa- 


JJ T U = UY? m 


(B.38) 
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if we define the matrix E m as 


E 


m 


Tl 

02 


a 


m 


Now, define 


V m = J T U E 


-l 


(B.39) 


and let V be any orthogonal matrix that satisfies V = [V rrL \ V n - m ] (note that 
here V n - m contains just enough columns so that the matrix V is an n x n 
matrix). It is a simple matter to combine the above equations to verify Equation 
(|R34|: 


uev t 


u [E m I 0] 


v T 1 

v m 

v T 

* n—m 


UY> m {J T U^) T 

C/E m (E- 1 ) T t/ T J 

[7E m E- 1 [/ T J 

UU T J 

J. 


(B.40) 

(B.41) 

(B.42) 

(B.43) 

(B.44) 

(B.45) 

(B.46) 


Here, Equation (B.40) follows immediately from our construction of the matri- 
ces U, V and E m . Eq uation ( B.42) is o btained by substituting Equation ( B.39 ) 
into Equation (B.41). Equation (B.44 1 follows because E” 1 is a diagonal ma- 
trix, and thus symmetric. Finally, Equation (B.46 1 is obtained using the fact 
that U T = C7 — 1 , since U is orthogonal. 



Appendix C 
Lyapunov Stability 


We give here some basic definitions of stability and Lyapunov functions and 
present a sufficient condition for showing stability of a class of nonlinear sys- 
tems. For simplicity we treat only time-invariant systems. For a more general 
treatment of the subject the reader is referred to [?]. 


Definition C.l Consider a nonlinear system on 

x = f(x) 


(C.l) 


where f{x) is a vector field on 
in R n is said to be an equilibrium point for (C.l). 


and suppose that /( 0) = 0. Then the origin 


If initially the system (C.l I satisfies x{tp) = 0 then the function x(to) = 0 for 
t > to can be seen to be a solution of ( |C.l I called the null or equilibrium 
solution. In other words, if the system represented by (C.l I starts initially at 


the equilibrium, then it remains at the equilibrium thereafter. The question of 


stability deals with the solutions of (C.l) for initial conditions away from the 


equilibrium point. Intuitively, the null solution should be called stable if, for 
initial conditions close to the equilibrium, the solution remains close thereafter 
in some sense. We can formalize this notion into the following. 
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Definition C.2 The null solution x(t ) = 0 is stable if and only if, for any 
e > 0 there exist 6(e) > 0 such that 

||a;(fo)|| < 6 implies ||x(t)|| < e for all t > to- (C.2) 

This situation is illustrated by Figure [Cd~| and says that the system is stable 



if the solution remains within a ball of radius e around the equilibrium, so long 
as the initial condition lies in a ball of radius 6 around the equilibrium. Notice 
that the required 6 will depend on the given e. To put it another way, a system 
is stable if “small” perturbations in the initial conditions, results in “small” 
perturbations from the null solution. 

Definition C.3 The null solution x(t) = 0 is asymptotically stable if and only 
if there exists 6 > 0 such that 

||x(to)|| < $ implies ||:r(f)|| — > 0 as t — > oo. (C.3) 

In other words, asymptotic stability means that if the system is perturbed away 
from the equilibrium it will return asymptotically to the equilibrium. The above 
notions of stability are local in nature, that is, they may hold for initial condi- 
tions “sufficiently near” the equilibrium point but may fail for initial conditions 
farther away from the equilibrium. Stability (respectively, asymptotic stability) 
is said to be global if it holds for arbitrary initial conditions. 

We know that a linear system 


x = Ax (C.4) 

will be globally asymptotically stable provided that all eigenvalues of the matrix 
A lie in the open left half of the complex plane. For nonlinear systems stability 
cannot be so easily determined. 
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Another important notion related to stability is the notion of uniform ul- 
timate boundedness of solutions. 


( C.l ) with initial condition 


Definition C.4 A solution x(t) : [to,oo] —* 
x(to) = xq is said to uniformly ultimately bounded (u.u.b.) with respect 
to a set S if there is a nonnegative constant T(xq,S) such that 


x(t) £ S for all t > to + T. 


Uniform ultimate boundedness says that the solution trajectory of ( C.l[ >) 
beginning at xq at time to will ultimately enter and remain within the set S. 
If the set S' is a small region about the equilibrium, then uniform ultimate 
boundedness is a practical notion of stability, which is useful in control system 
design. 


C.0.1 Quadratic Forms and Lyapunov Functions 

Definition C.5 Given a symmetric matrix P = (pij) the scalar function 

n 

V(x) = x T Px= ^ pijXiXi (C.5) 

i,j = i 

is said to be a quadratic form. V(x), equivalently the quadratic form, is said 
to be positive definite if and only if 

V(x) > 0 (C.6) 


for 

Note that V(0) = 0. V(x) will be positive definite if and only if the matrix P 
is a positive definite matrix, that is, has all eigenvalues positive. 

The level surfaces of V, given as solutions of V(x) = constant are ellipsoids 
in R". A positive definite quadratic form is like a norm. In fact, given the usual 
norm ||a;|| on R n , the function V given as 

V(x) = x T x = ||a;|| 2 (C.7) 


is a positive definite quadratic form. 


Definition C.6 Let V{x) : R" — > R be a continuous function with continuous 
first partial derivatives in a neighborhood of the origin in R”. Further suppose 
that V is positive definite, that is, V(0) = 0 and V > 0 for x 0. Then V is 
called a Lyapunov Function Candidate (for the system (C.l). 


The positive definite function V is also like a norm. For the most part we 
will be utilizing Lyapunov function candidates that are quadratic forms, but 
the power of Lyapunov stability theory comes from the fact that any function 
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may be used in an attempt to show stability of a given system provided it is a 
Lyapunov function candidate according to the above definition. 

By the derivative of V along trajectories of ( C. 1 ) , or the derivative of V in 


the direction of the vector field defining (C.l), we mean 


V(t) = 


dV 

(dVJ) = —h(x) 


dV 

dx n 


fn( X). 


(C.8) 


Suppose that we evaluate the Lyapunov function candidate V at points along 


a solution trajectory x(t) of ( C.l I and find that V ( t ) is decreasing for increasing 
t. Intuitively, since V acts like a norm, this must mean that the given solution 
trajectory must be converging toward the origin. This is the idea of Lyapunov 
stability theory. 


C.0.2 Lyapunov Stability 


Theorem 6 The null solution of (C.l) is stable if there exists a Lyapunov func- 


tion candidate V such that V is negative semi-definite along solution trajectories 


of (C.l ), that is, if 


V = (dVJ(x)) = dV T f(x)< 0. 


(C.9) 


Equation (C.9 1 says that the derivative of V computed along solutions of 
is nonpositive, which says that V itself is nonincreasing along solutions. Since 


V is a measure of how far the solution is from the origin, (C.9 1 says that the 
solution must remain near the origin. If a Lyapunov function candidate V 


can be found satisfying (C.9 1 then V is called a Lyapunov Function for the 
system (C.l ). Note that Theorem [6] gives only a sufficient condition for stability 
of (C.l). If one is unable to find a Lyapunov function satisfying (C.9 1 it does 


not mean that the system is unstable. However, an easy sufficient condition for 


instability of (C.l) is for there to exist a Lyapunov function candidate V such 


that V > 0 along at least one solution of the system. 


Theorem 7 The null solution of (C.l) is asymptotically stable if there exists 
a Lyapunov function candidate V such that V is strictly negative definite along 


solutions of (C.l ), that is, 


V(x) < 0. 


(C.10) 


The strict inequality in (C.10) means that V is actually decreasing along solu- 
tion trajectories of (C.l I and hence the trajectories must be converging to the 
equilibrium point. 

Corollary C.l Let V be a Lyapunov function candidate and let S be any level 
surface of V, that is, 


S(c 0 ) = { s er|l/( a: )=co} 


(C.ll) 
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Fig. C.2 Illustrating ultimate boundedness. 


for some constant Co > 0. 
bounded with respect to S 


Then a solution x(t) of (C.l ) is uniformly ultimately 

if 


V = (dVJ (*))<0 


(C.12) 


for x outside of S. 

If V is negative outside of S then the solution trajectory outside of S must be 
pointing toward S as shown in Figure |C.2| Once the trajectory reaches S we 
may or may not be able to draw further conclusions about the system, except 
that the trajectory is trapped inside S. 


C.0.3 Lyapunov Stability for Linear Systems 

Consider the linear system and let 

V(x) = x T Px (C.13) 


be a Lyapunov function candidat e, wh ere P is symmetric and positive definite. 
Computing V along solutions of ( C.4 ) yields 


V = x T Px + x T Px (C.14) 

= x T {A T P + PA)x 
= —x T Qx 


where we have defined Q as 


A t P + PA = -Q. 


(C.15) 


Theorem C.8 now says that if Q given by (C.15 1 is positive definite (it is 
automatically symmetric since P is) then the linear system (C.4) is stable. One 
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approach that we can now take is to first fix Q to be symmetric, positive definite 


and solve (C.15), which is now called the matrix Lyapunov equation, for P. 


If a s ymmetric positive definite solution P can be found to this equation, then 
( C.4 ) is stable and x T Px is a Lyapunov function for the linear system ( C.4 1 . 


The converse to this statement also holds, 
statements as 


In fact, we can summarize these 


Theorem 8 Given an n x n matrix A then all eigenvalues of A have negative 
real part if and only if for every symmetric positive definite nxn matrix Q, the 


Lyapunov equation (C.ll) has a unique positive definite solution P. 


Thus, we can reduce the determination of stability of a linear system to the 
solution of a system of linear equations, namely, (C.ll I, which is certainly 


easier than finding all the roots of the characteristic polynomial and, for large 
systems, is more efficient than, say, the Routh test. 


The strict inequality in ( C.7 1 may be difficult to obtain for a given system 
and Lyapunov function candidate. We therefore discuss LaSalle’s Theorem 
which can be used to prove asymptotic stability even when V is only negative 
semi-definite. 


C.0.4 LaSalle’s Theorem 


Given the system (C.l I suppose a Lyapunov function candidate V is found such 


that, along solution trajectories 


V <0. 


(C.16) 


Then (C.ll is asymptotically stable if V does not vanish identically along any 
solution of (C.ll other than the null solution, that is, (C.ll is asymptotically 
stable if the only solution of (C.l I satisfying 


V = 0 


(C.17) 


is the null solution. 


Appendix D 

State Space Theory of 
Dynamical Systems 


Here we give a brief introduction to some concepts in the state space theory of 
linear and nonlinear systems. 

Definition D.l A vector field f is a continuous function f : IR n — > IR n . 


We can think of a differential equation 

x{t) = f{x{t)) 


(D.l) 


as being defined by a vector field / on IR n . A solution t — > x{t) of (D.l) with 
x{t o) = xq is then a curve C in IR n , beginning at xq parametrized by t, such 
that at each point of C, the vector field f(x(t)) is tangent to C . IR n is then 
called the state space of the system GID. For two dimensional systems, we 
can represent 


Xl (t) 
X2 (t) 


(D.2) 


by a curve C in the plane. 
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*2 



Fig. D.l Phase portrait for Example B.l 


Example D.l Consider the two-dimensional system 

xi=x 2 £i(0)=xio 

X 2 = ~X\ X 2 (tS) = X 20 

In the phase plane the solutions of this equation are circles of radius 


'10 ' 


b 20 


To see this consider the equation 


Oi(t)+xl (t) = r. 


(D.3) 

(D.4) 

(D.5) 

(D.6) 


Clearly the initial conditions satisfy this equation. If we differe ntiat e \D.O ) in 
the direction of the vector field f = (x 2 ,—Xi) t that defines (D.S)-(D.f) we 
obtain 


2cEii:i + 2x 2 x 2 = 2x\x 2 — 2x 2 x\ = 0. 


(D.7) 


Thus f is tangent to the circle. The graph of such curves C in the X\ — x 2 plane 
for different initial conditions are shown in Figure \ D.l\ o 

The X\ — x 2 plane is called the phase plane and the trajectories of the 
system ( D.3 )-( D.4 ) form what is called the phase portrait. For linear systems 
of the form 


x = Ax (D.8) 

in R 2 the phase portrait is determined by the eigenvalues and eigenvectors of 
A . For example, consider the system 


Xl = x 2 
x 2 = X\. 


(D.9) 

(D.10) 
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Fig. D.2 Phase portrait for Example B.2. 


In this case 


A 


0 1 
1 0 


(D.ll) 


The phase portrait is shown in Figure |D.2| The lines £\ and £2 are in the 
direction of the eigenvectors of A and are called eigen-subspaces of A. 


D.0.5 State Space Representation of Linear Systems 

Consider a single-input /single-output linear control system with input u and 
output y of the form 

r{ n 7 / r\ n ^ 7 / rjii 

an dt" +an - 1 dt^ + '" + a + 1 Tt +aoy = u ' (D - 12) 

The characteristic polynomial, whose roots are the open loop poles, is given as 

p(s ) = a n s n + a n _is n_1 -I ha 0 . (D.13) 


For simplicity we suppose that p{x) is monic, that is, a n = 1. The stan- 
dard way of representing (D.12) in state space is to define n state variables 
xi,x 2 , ■■■,x n as 


Xi 

= y 

X 2 

= y = x 1 

X3 

= y = xi 


d n ~ 1 y 

X n 

dt n ~ l 


— %n— 1 


(D.14) 
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and 


express (D.12l as 


the system of first order differential equations 


*i 
X'2 
Xn— 1 

X n 


X 2 (D.15) 

x-i 

Xn 

d n y dy d n - y 

dE = + “ 

CLqX\ d\X 2 * * * ~~ I - d. 


In matrix form this system of equations is written as 


Xi 




0 

1 • 

0 




' 0 ' 

0 

0 1 

0 


Xi 

+ 

0 



1 




0 

— a 0 


* Q"n— 1 




1 


(D.16) 


or 


x = Ax + bu x £ R ra . 


The output y can be expressed as 


V = [1,0,..., 0]a 


T 

= C X. 


It is easy to show that 

det (si — A) = s n + a n _i5 n_1 + 


+ d\S + CLq 


(D.17) 


(D.18) 


and so the last row of the matrix A consists of precisely the coefficients of the 
characteristic polynomial of the system, and furthermore the eigenvalues of A 
are the open loop poles of the system. 

y( s \ 

In the Laplace domain, the transfer function jjjdj is equivalent to 


Y{s) 

U(s) 


c T (sI — A)~ 1 b. 


(D.19) 
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Across Variable, 287 
Actuator Dynamics, 231 
Angle, 69 

Angular momentum, 320, 323 
conservation of, 299 
Angular velocity, 119 
Anthropomorphic, 10 
Anti-windup, 260 
Apparent Damping, 293 
Apparent Inertia, 293 
Apparent Stiffness, 293 
Application area, 6 
Approach, 74 
Arbitrary, 255 
Arm, 7 

Armature, 231 

Arm singularity, 133 

Articulated (RRR), 6 

Artificial Constraint, 284-285 

Assembly, 6 

Atan2, 48 

Atan, 377 

Average, 237 

Axis/ Angle, 47 

Axis/angle representation, 52 

Back emf, 231 

Back emf Constant, 233 

Bang-Bang, 182 

Base, 18 


Basic homogeneous transformation, 55 

Basic rotation matrix, 36 

Basis, 283 

Bilinear Form, 283 

Blend time, 179 

Capacitive, 288 

Capacitive Environment, 288 

Cartesian (PPP), 6 

Centrifugal, 202 

Characteristic polynomial, 307 

Chow’s theorem, 299, 325 

Christoffel symbol, 201 

Closed-form equations, 216 

Closing, 9 

Codistribution, 301 

Compensator, 230 

Completely integrable, 305 

Completely observable, 257 

Completely state-controllable, 254 

Compliance Frame, 284 

Computed torque, 23, 267 

Computer interface, 7 

Configuration, 4 

Configuration kinematic equation, 68 

Configuration space, 4 

Conservation of angular momentum, 323 

Constraint, 282 

Constraint Frame, 284 

Constraint 

Holonomic, 187, 191 


403 



404 INDEX 


Constraint 

holonomic, 319 
Constraint 

Nonholonomic, 191 
Constraint 

nonholonomic, 299 
Pfaffian, 319 
rolling, 320 
Constraints 
Holonomic, 188 
Continous Path Tracking, 229 
Continuous path, 6 
Control, 1 

Control computer, 7 
Control 

Independent Joint, 230 
Control 

inner loop/outer loop, 307 
Control 

inverse dynamic, 267 
Controllability, 254 
Controllability, 325 
Controllability matrix, 254 
Controllability rank condition, 325 
Controllable, 254 
Controller resolution, 8 
Control 

outer loop, 307 
Coordinates 

Generalized, 190 
Generalized, 192 
Coriolis, 202 
Cotangent space, 300 
Covector field, 300 
Cross-product form, 22 
Current 
Armature, 233 
Current frame, 43, 46 
Cylindrical (RPP), 6 
D’Alembert’s Principle, 194 
Damping 
Apparent, 293 
DC-gain, 288 
DC-Motor, 189 
Decoupled, 211 
Degrees-of- freedom, 4 
Denavit-Hartenberg, 19 
Dexterous workspace, 5 
Diffeomorphism, 300, 309, 317 
Direct Drive Robot, 230 
Directional derivative, 303 
Displacement 
Virtual, 192 
Distribution, 301 
Involutive, 306 
Disturbance, 230 
Disturbance Rejection, 230 


Double integrator system, 267 
Driftless system, 320, 325 
Driftless systems, 299 
Dual vector space, 300 
Dynamics, 1, 187 
In task space, 291 
Newton-Euler Formulation, 188 
Effective inertia, 237 
Effort, 287 
End-effector, 9, 74 
End-of-arm tooling, 7 
Energy, 287 
Kinetic, 187-188 
Potential, 187, 189 
Environment, 281 
Capacitive, 288 
Classification of, 293 
Inertial, 288 
Resistive, 288 
Environment Stiffness, 286 
Equation 

Euler-Lagrange, 187 
Euler-Lagrange, 188 
Estimation error, 257 
Euler Angle, 47 
Euler Angles, 47, 192 
Euler-Lagrange equation, 188 
External and internal sensor, 7 
External power source, 7 
Eye- in- hand configuration, 356 
Feedback linearizable, 308 
Feedback linearization, 299 
global, 314 

Feedforward control, 23, 244 

Five-bar linkage, 209 

Fixed-camera configuration, 356 

Fixed frame, 44, 46 

Flow, 287 

Force, 281 

Force control, 24 

Force Control, 281 

Force 

Generalized, 190 
Generalized, 195 
Gravitational, 189 
Forward, 68 

Forward kinematic equation, 19 
Forward kinematics problem, 18 
Frame 

compliance, 284 
constraint, 284 
Frame 

current, 46 
fixed, 46 

Frobenius theorem, 304, 311 
Gear Train, 189 

Generalized Coordinates, 190, 192 
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Generalized Force, 190, 195 
Geometric nonlinear control, 299 
Global feedback linearization, 309 
Gradient, 303 
Group, 59 

Guarded motion, 171 
Gyroscopic term, 217 
Hand, 9 

Hardware/Software trade-off, 229 
Holonomic constraint, 319 
Home, 17 

Homogeneous coordinate, 19 
Homogeneous representation, 55 
Homogeneous transformation, 19, 55 
Hybrid control, 24 
Hybrid Impedance Control, 293 
Image-based visual servo control, 357 
Image feature velocity, 358 
Image Jacobian, 358 
Impedance, 286 
Impedance, 288 
Impedance control, 24 
Impedance Control, 292 
Hybrid, 293 
Impedance 
Dual, 294 

Impedance Operator, 288 
Implicit function theorem, 305 
Independent Joint Control, 230 
Inductance 
Armature, 233 
Inertia 

Apparent, 293 
Inertial, 288 

Inertial Environment, 288 
Inertia matrix, 200 
Inertia Tensor, 197 
Inner-loop, 269 
Inner loop control, 299 
Inner product, 303 
Integrability 
complete, 305 

Integral manifold, 304-305, 324 
Integrator windup, 260 
Interaction matrix, 358-359 
Invariant, 283 
Inverse dynamics, 23 
Inverse Dynamics, 291 
Inverse dynamics, 299 
Inverse dynamics control, 267 
Inverse Kinematics, 20 
Inverse kinematics, 85 
Inverse orientation kinematic, 87 
Inverse position kinematic, 87 
Involutive, 306 
Involutive closure, 324 
Jacobian, 22, 113, 123, 290 


Jacobian matrix, 302 
Joint flexibility, 299, 311 
Joints, 3 

Joint torque sensor, 282 
Joint variable, 4 
Killing Form, 283 
Kinematically redundant, 4 
Kinematic chain, 3 
Kinematics, 1 
Kinetic Energy, 187-188 
Klein Form, 283 
Lagrangian, 187, 189, 196 
Laplace Domain, 288 
Laplace Transform, 288 
Law of Cosines, 20 
Left arm, 91 
Length, 69 
Lie bracket, 302, 306 
Lie derivative, 303 

Linear Quadratic (LQ) Optimal Control, 256 
Linear Segments with Parabolic Blends, 177 
Linear state feedback control law, 254 
Links, 3 
LSPB, 177 
Magnetic Flux, 231 
Manifold, 300, 302 
Manipulation, 320 
Manipulator Jacobian, 123 
Manipulator 
spherical, 11 

Matrix Algebraic Riccatti equation, 256 
Matrix 

inertia, 200 
transformation, 67 
Mechanical Impedance, 286 
Method of computed torque, 247 
Method of control, 6 
Minimum phase, 245 
Mobile robot, 320 
Mobile robots, 299 
Mobility Tensor, 292 
Motion 

guarded, 171 
Motion pereptibility, 372 
Motor 
AC, 231 

Brushless DC, 231 
DC, 230-231 
Rotor, 231 
Stator, 231 

Natural Constraint, 284-285 
Network Model, 287 
Newton-Euler formulation, 215 
Newton’s Second Law, 188 
Non- assembly, 6 

Nonholonomic constraint, 299, 319 
Non-servo, 6 
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Normal, 74 

Norton Equivalent, 289 
Norton Network, 289 

Numerically controlled milling machines, 2 

Observability, 257 

Observability matrix, 258 

Observable, 257 

Observer, 256-257 

Offset, 69 

One-Port, 287 

One-Port Network, 287 

Opening, 9 

Operator 

Impedance, 288 
Orientation, 4 
Orientation matrix, 19 
Orientation of the tool frame, 19 
Orthogonal, 34, 284 
Orthonormal Basis, 284 
Outer-loop, 269 
Outer Loop, 291 
Outer loop control, 299, 307 
Parallelogram linkage, 10 
Partitioned methods, 369 
Permanent magnet, 232 
Permanent Magnet DC Motor, 230 
Perspective projection, 334 
Pfaffian constraint, 319 
Pitch, 49 
Planning, 1 
Point-to-point, 6 
Point to point, 171 
Point-to-Point Control, 229 
Port Variable, 287 
Position, 281 

Position-based visual servo control, 357 

Positioning, 4 

Post-multiply, 46 

Potential Energy, 187, 189 

Power, 287 

Power source, 6 

Premultiply, 46 

Principle 

D’Alembert, 194 

Principle of Virtual Work, 188, 193 
Prismatic, 3 
Quaternion, 61 
Reachable workspace, 5 
Reciprocal Basis, 283 
Reciprocity, 284 
Reciprocity Condition, 285 
Rejecting, 23 
Repeatability, 7 
Representation 
axis/ angle, 52 
homogeneous, 55 
Resistance 


Armature, 233 
Resistive, 288 

Resistive Environment, 288 

Resolvability, 372 

Reverse order, 44 

Re volute, 10, 3 

Right arm, 91 

Robot, 1 

Robota, 1 

Robot 

Direct Drive, 230 
Robot 

flexible joint, 311 
Robotic System, 7 
Robot Institute of America, 2 
Robot 
mobile, 299 
mobile, 320 
Roll, 49 

Rolling constraint, 320 

Rolling contact, 299 

Roll-pitch-yaw, 47 

Rotation matrix, 33 

Rotor, 231 

Satellite, 326 

Saturation, 242 

SCARA, 12 

SCARA (RRP), 6 

Second Method of Lyapunov, 23 

Separation Principle, 258 

Servo, 6 

Set-point tracking problem, 238 
Singular configuration, 22, 132 
Singular configurations, 114 
Singularity, 132 
Skew symmetric, 115 
Sliding, 74 

Spherical manipulator, 11 

Spherical (RRP), 6 

Spherical wrists, 8 

State, 5 

State space, 5 

Stator, 231 

Stiffness 

Apparent, 293 
Strain gauge, 282 
Switching time, 182 
Symbol 

Christoffel, 201 
System 

double integrator, 267 
System 

driftless, 320 
driftless, 325 
Tactile sensor, 282 
Tangent plane, 305 
Tangent space, 300, 324 
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Task Space, 290 

Task Space Dynamics, 291 

Teach and playback mode, 171 

Teleoperators, 2 

Theorem 

Frobenius, 304 
Thevenin Equivalent, 289 
Thevenin Network, 289 
Through Variable, 287 
Tool frame, 74 
Torque 

computed, 267 
Torque Constant, 233 
Track, 23 
Tracking, 230 

Tracking and Disturbance Rejection 
Problem, 23 
Trajectory, 171 
Trajectory Control, 281 
Transformation 

basic homogeneous, 55 
homogeneous, 55 
Transformation matrix, 67 
Transpose, 290 
Twist, 69 

Two-argument arctangent function, 48 
Two link manipulator, 290 


Unicycle, 320 
Vector field 
complete, 324 
smooth, 300 
Velocity 

angular, 119 
Via points, 171 
Via points, 182 
Virtual displacement, 192 
Virtual Displacement, 290 
Virtual Displacements, 188 
Virtual Work, 187, 290 
Vision, 1 
Voltage 

Armature, 233 
Workspace, 5 
dexterous, 5 
reachable, 5 
Work 

Virtual, 187 
Virtual, 290 
World, 18 
Wrist, 8 
Wrist center, 87 
Wrist force sensor, 282 
Wrist singularity, 133 
Yaw, 49 



